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1.1 智能 机 器 人 概念 及 其 特点 


1.1.1 智能 机 器 人 概念 


1954 年 ， 美 国人 乔治 - 德 沃 尔 制造 出 了 世界 上 第 一 台 可 编程 的 机 器 人 ; 1959 年 ， 德 沃 尔 与 美国 发 明 家 约瑟夫 ` 英 格 伯 格 联手 制造 出 了 第 一 台 工 业 机 器 人 ， 自 此 之 后 ， 机 器 人 大 体 经 历 了 如 下 三 个 发 展 阶 
段 。 第 一 代 机 器 人 称 为 示 教 再 现 型 机 器 人 ， 它 是 通过 计算 机 控制 一 个 多 自由 度 的 机 械 ， 并 通过 示 教 存储 程序 和 信息 ， 在 工作 时 读 取信 息 ， 发 出 指令 ， 机 器 人 就 可 以 根据 事先 示 教 的 结果 ， 再 现 出 同样 的 动 
作 ， 如 汽车 点 焊 机 器 人 只 要 对 其 示 教 点 焊 过 程 ， 它 就 总 是 重复 同样 的 操作 ， 不 过 因为 其 对 于 外 界 环境 没有 感知 ， 因 而 操作 力 大 小 、 点 焊 质量 等 均 无 从 知晓 。 第 二 代 机 器 人 称 为 带 感觉 的 机 器 人 ， 通 过 为 其 配 
置 各 类 传感器 ， 其 具有 一 定 程度 的 力 觉 、 触 党 、 视 觉 等 感 沉 能力， 如 当 搬 运 机 器 人 抓 住 一 样 物体 时 ， 它 就 能 自动 感觉 出 实际 的 用 力 大 小 ， 并 能 通过 视觉 感受 和 识别 出 物体 的 形状 、 大 小 及 其 颜色 。 第 三 代 机 
器 人 即 智能 机 器 人 ， 目 标 是 只 需要 告诉 它 做 什么 ， 而 不 用 告诉 它 怎么 做 ， 它 就 能 完成 感知 、 运 动 、 推 理 和 人 机 交互 等 功能 和 机 能 ， 目 前 还 只 在 局 部 有 这 种 智能 的 概念 和 含义 ， 随 着 科学 技术 的 不 断 发 展 ， 智 
能 的 概念 与 内 涵 将 会 越 来 越 丰 语 。 

到 目前 为 止 ， 在 世界 范围 内 还 没有 一 个 统一 的 智能 机 器 人 的 定义 。1956 年 ， 马 文 - 明 斯 基 提 出 了 他 对 智能 机 器 的 理解 : “智能 机 器 能 够 创建 周围 环境 的 抽象 模型 一旦 遇 到 问题 ， 便 能 够 从 抽象 模型 中 寻 
找 解 决 方法 ”。 该 定义 对 此 后 30 年 智能 机 器 人 的 研究 方向 产生 了 重要 影响 。 


还 有 一 种 理解 是 智能 机 器 人 是 具备 某 些 与 生物 类 似 的 智能 能 力 且 具有 高 度 灵 活性 的 自动 化 机 器 。 大 多 数 专 家 认为 智能 机 器 人 至 少 要 具备 以 下 三 个 要 素 : 一 是 感 沉 要素 ， 用 来 认 知 周围 的 环境 状态 ; 二 是 
运动 要 素 ， 用 来 对 外 界 做 出 反应 性 动作 ; 三 是 思考 要 素 ， 根 据 感 党 要 素 所 得 到 的 信息 ， 思 考 应 采用 什么 样 的 动作 。 

国内 科学 家 对 机 器 人 的 定义 是 : “智能 机 器 人 是 一 种 自动 化 的 机 器 ， 所 不 同 的 是 这 种 机 器 具备 一 些 与 人 或 生物 相似 的 智能 能 力 。” 在 研究 和 开发 作业 于 未 知 及 不 确定 环境 下 的 机 器 人 的 过 程 中 ， 人 们 逐 
步 认 识 到 机 器 人 技术 的 本 质 是 感知 、 决 策 、 行 动 和 交互 技术 的 结合 ， 因 此 将 具有 感知 、 思 考 、 决 策 和 动作 的 技术 系统 统称 为 智能 机 器 人 ， 其 是 具有 感知 能 力 、 规 划 能 力 、 动 作 能 力 和 协同 能 力 的 一 种 高 度 灵 
活性 的 自动 化 机 器 。 
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1.1.2 ”智能 机 器 人 的 友 展 


1. 智 能 机 器 人 的 发 展 背景 


机 械 的 使 用 放大 并 延伸 了 人 的 四 肢 能 力 ， 计 算 机 的 使 用 提升 了 人 脑 的 功能 ， 而 机 器 人 的 使 用 将 大 大 拓展 人 类 的 综合 能 力 。 进 入 21 世 纪 以 来 ， 智 能 机 器 人 的 研发 和 应 用 得 以 快速 发 展 ， 其 主要 发 展 背 景 
含 以 下 几 个 方面 : 


1) 制造 业 劳动 力 的 价格 越 来 越 高 ， 而 产品 的 价格 却 越 来 越 低 ， 企 业 需要 利用 机 器 人 改变 传统 制造 业 依赖 密集 型 廉价 劳动 力 的 生产 模式 。 

2) 人 类 寿命 和 生活 质量 越 来 越 高 ， 然 而 能 够 提供 老龄 化 服务 的 人 力 资源 却 越 来 越 少 ， 因 此 需要 利用 智能 化 机 器 设备 提供 优质 服务 。 

3) 自然 灾害 、 人 为 灾害 及 战争 仍 在 频繁 友 生 ， 而 人 类 难以 适应 此 类 环境 ， 因 此 需要 机 器 人 代替 人 来 执行 任务 。 

4) 人 类 探索 深海 、 太 空 等 极端 环境 的 活动 越 来 越 频 繁 ， 而 人 类 在 此 类 环境 中 的 生存 能 力 极 低 且 代价 很 高 ， 因 此 需要 利用 机 器 人 实现 远程 交互 作业 。 
2. 初 级 与 高 级 智能 机 器 人 


智能 机 器 人 正在 由 初级 阶段 向 高 级 阶段 发 展 。 初 级 智能 机 器 人 具有 像 人 那样 的 感受 、 识 别 、 推 理 和 判断 的 能 力 ， 可 以 根据 外 界 条 件 的 变化 ， 在 一 定 范围 内 自行 修改 程序 ， 也 就 是 它 能 适应 外 界 条 件 的 变 
化 并 对 自己 进行 相应 的 调整 。 不 过 ， 修 改 程序 的 原则 将 由 人 预先 给 以 规定 。 这 种 初级 智能 机 器 人 已 拥有 一 定 的 智能 水 平 ， 虽 然 还 没有 自动 规划 的 能 力 ， 但 这 种 初级 智能 机 器 人 已 走向 成 熟 ， 达 到 了 实用 的 水 
F. 

高 级 智能 机 器 人 和 初级 智能 机 器 人 一 样 ， 具 有 感觉 、 识 别 、 推 理 和 判断 的 能 力 ， 同 样 可 以 根据 外 界 条 件 的 变化 ， 在 一 定 范围 内 自行 修改 程序 。 所 不 同 的 是 ， 修 改 程 序 的 原则 不 是 由 人 预先 规定 的 ， 而 是 
机 器 人 自己 通过 学 习 、 总 结 经 验 来 获得 修改 程序 的 原则 ， 所 以 它 的 智能 程度 更 高 。 这 种 机 器 人 已 拥有 一 定 的 自动 规划 能 力 ， 能 够 自己 安排 自己 的 工作 。 这 种 机 器 人 可 以 不 要 人 的 照料 ， 完 全 独立 地 工作 ， 故 
称 为 高 级 自律 机 器 人 ， 这 类 机 器 人 也 开始 走向 实用 


3. 特 种 智能 机 器 人 


特种 智能 机 器 人 具有 较 大 的 应 用 前 景 。 随 着 人 们 对 机 器 人 技术 智能 化 本 质 认 识 的 加 深 ， 人 们 开发 出 了 各 式 各 样 的 具有 感知 、 决 策 、 行 动 和 交互 能 力 的 特种 智能 机 器 人 ， 如 水 下 机 器 人 、 空 间 机 器 人 、 空 
中 机 器 人 、 地 面 机 器 人 、 微 小 型 机 器 人 等 各 种 用 途 的 机 器 人 相继 问世 。 对 不 同 任务 和 特殊 环境 的 自 适 应 性 ， 也 是 智能 机 器 人 与 一 般 自动 化 装备 的 重要 区 别 。 这 些 机 器 人 从 外 观 上 已 远 远 脱离 了 最 初 仿 人 型 机 
器 人 和 工业 机 器 人 所 有 具有 的 形状 ， 而 是 更 加 符合 各 种 不 同 应 用 领域 的 特殊 要 求 ， 其 功能 和 智能 程度 也 大 大 增强 ， 从 而 为 机 器 人 技术 开辟 出 更 加 广阔 的 发 展 空间 。 


将 智能 机 器 人 的 技术 (如 传 感 技术 、 智 能 技术 、 控 制 技术 等 ) 扩散 和 渗透 到 各 个 领域 形成 各 式 各 样 的 新 机 器 一 一 机 器 人 化 机 器 。 当 前 与 信息 技术 的 交互 和 融合 又 产生 了 “软件 机 器 人 ” “网络 机 器 
人 ”等 名 称 ， 这 也 说 明了 机 器 人 所 具有 的 创新 活力 。 


1.1.3 ”智能 机 器 人 的 特点 


智能 机 器 人 的 特点 就 是 智能 ， 其 具体 体现 在 以 下 几 方 面 。 

(AZM: 是 指 其 可 以 在 特定 的 环境 中 ， 不 依赖 任何 外 部 控制 ， 无 需 人 为 干预 ， 完 全 自主 地 执行 特定 的 任务 。 

. 适应 性 : 是 指 它 可 以 实时 识别 和 测量 周围 的 物体 ， 根 据 环 境 的 变化 调节 自身 的 参数 ， 调 整 动 作 策略 ， 以 及 处 理 紧 和 急 情 况 。 
RAV: 是 指 机 器 人 可 以 与 人 、 外 部 环境 及 与 其 他 机 器 人 之 间 进 行 信息 交流 。 

. 学 习性 : 是 指 机 器 人 在 自主 感知 环境 变化 的 基础 上 ， 可 以 形成 和 进化 出 新 的 活动 规则 ， 自 主 独 立地 活动 和 处 理 问 题 。 

. 协同 性 : 是 指 在 实时 交互 的 基础 上 ， 机 器 人 可 以 依据 需求 和 任务 实现 机 机 协作 和 人 机 协同 。 


智能 机 器 人 技术 是 综合 了 计算 机 、 控 制 论 、 机 构 学 、 信 息 和 传 感 技术 、 人 工 智 能 、 仿 生 学 等 多 学 科 而 形成 的 高 新 技术 ， 是 当代 研究 十 分 活跃 ， 应 用 日 益 广泛 的 领域 。 智 能 机 器 人 应 用 是 衡量 一 个 国家 工 
业 化 和 信息 化 发 展 水 平 的 重要 标志 。 


1.2 智能 机 器 人 的 组 成 


1.2.1 智能 机 器 人 的 硬件 组 成 


智能 机 器 人 所 具备 的 感知 、 规 划 、 动 作 和 协同 等 能 力 确定 了 其 硬件 的 组 成 与 结构 。 为 了 感知 和 识别 对 象 和 环境 ， 其 需要 丰富 的 内 部 传感器 和 外 部 传感器 ;为 了 作用 于 对 象 并 影响 周围 环境 ， 其 需要 多 种 
能 力 的 动作 执行 器 ; 智能 机 器 人 的 核心 是 一 个 相当 发 达 的 “大 脑 ” 一 一 控制 计算 机 ， 其 用 于 实现 机 器 人 的 实时 管控 和 智能 处 理 ; 智能 机 器 人 具有 人 机 协同 、 多 机 器 人 协作 等 能 力 ， 因 此 需要 配置 多 样 化 交互 


接口 和 网 络 通信 接口 。 综 上 所 述 其 硬件 组 成 如 图 1-1 所 示 。 
(1) 内 部 传感器 组 
完成 机 器 人 自身 位 置 、 速 度 、 力 度 等 实时 测量 和 感知 ， 为 实现 伺服 控制 提供 依据 。 


AE (位 移 ) 传感器 : 感知 机 器 人 当前 位 置 和 位 移 状 态 ， 其 又 可 分 为 直线 移动 传感器 和 角 位 移 传感器 ; 例如 ， 实 现 角 位 移 感知 的 光电 编码 器 能 够 得 到 对 应 于 编码 器 初始 锁定 位 置 的 驱动 轴 有 瞬时 角度 
值 。 当 设备 感受 到 压力 时 ， 读 出 每 个 关节 编码 器 的 读数 ， 就 能 及 时 调整 伺服 控制 的 给 定 值 ， 以 防止 机 器 人 启动 时 产生 过 于 剧烈 的 运动 。 





图 1-1 ”智能 机 器 人 的 硬件 组 成 与 基本 结构 
速度 和 加 速度 传感器 : 主要 用 于 感知 旋转 运动 的 速度 ， 常 用 光电 脉冲 式 转速 传感器 来 实现 ， 加 速度 传感器 可 用 于 测量 机 器 人 的 动态 控制 信号 ， 一 般 由 速度 测量 进行 推演 。 主 流 的 传感器 有 应 力 传感器 
和 振动 式 加 速度 传感器 。 

ARG RE: 主要 用 于 测量 两 个 物体 之 间 的 作用 力 分 量 和 力 撼 分量。 主要 有 半导体 型 力 觉 传感器 、 转 抵 传 感 器 及 基于 弦 振 动 原 理 的 力 觉 传感器 等 。 人 例如， 斯坦福 研 究 所 研制 的 光 腕 力 传感器 是 由 6 个 小 
型 差 动 变压器 组 成 的 ， 能 够 测量 作用 于 腕 部 X、Y 和 忆 三 个 方向 的 动力 及 各 轴 动 转 抵 。 

(2) 外 部 传感器 组 

通过 视觉 、 触 觉 及 声 觉 等 的 感知 ， 实 现 智能 机 器 人 及 时 反映 环境 变化 、 自 主 调整 自身 行为 的 能 


` 视觉 传感器 : 机 器 视觉 已 成 为 智能 机 器 人 的 主要 感知 手段 ， 微 型 化 智能 数字 摄像 机 是 其 主要 选择 。 


` 触觉 传感器 : 用 于 感知 动作 执行 器 与 对 象 的 接触 及 其 程度 ; 微型 开关 是 触觉 传感器 的 常用 选择 ， 隔 离 式 双 态 接触 传感器 、 适 阵 传感器 、 光 反射 触觉 传感器 等 也 是 新 发 展 的 触觉 传感器 。 


* 声 觉 传 感 器 : 用 于 感受 和 解释 气体 〈 非 接触 感受 ) 、 液 体 或 固体 (接触 感受 ) 中 的 声波 。 上 声波 传感器 可 以 从 简单 的 声波 存在 检测 到 复杂 的 声波 频率 分 析 ， 直 到 对 连续 自然 语言 中 单独 语音 和 词汇 的 六 


于 检测 物体 的 滑动 。 当 要 求 智能 机 器 人 抓 住 特性 未 知 的 物体 时 ， 必 须 确定 最 适当 的 握力 值 ， 所 以 要 事先 检测 出 握力 不 够 时 所 产生 的 物体 滑动 信号 。 目 前 已 有 基于 光学 和 利用 晶体 接收 
器 等 不 同 原理 的 滑 沉 传感器 ， 后 者 的 检测 灵敏 度 与 滑动 方向 无 关 。 


; 用 于 感知 环境 温度 的 接触 式 或 非 接 触 式 温 度 传 感 器 ; 用 于 检测 物体 的 存在 和 测量 距离 的 超声 波 接 近 度 传 感 


z 


- 其 他 外 传感器 : 用 于 物 能 移动 机 器 人 的 距离 感知 的 激光 测 距 仪 、 声 呐 传 感 器 等 距离 传 感 呈 


(3) 驱动 与 作 动 器 
驱动 与 作 动 器 是 驱动 智能 机 器 人 活动 的 动力 机 构 。 驱 动 器 可 将 电能 、 液 压 能 和 和 气压 能 转化 为 机 器 人 的 动力 ， 并 驱动 自身 移动 或 机 械 臂 关节 的 移动 。 


驱动 电动 机 是 智能 机 器 人 的 基本 驱动 器 : 机 器 人 驱动 器 的 本 质问 题 是 控制 电动 机 ， 控 制 电动 机 的 转 数 就 可 以 控制 机 器 人 移动 的 距离 和 方向 、 机 械 劈 的 谊 曲 度 或 移动 距离 等 ， 驱 动 电 动机 主要 有 步 进 
动机 和 伺服 电动 机 。 步 进 电 动机 是 通过 给 内 部 多 个 线圈 依次 施加 脉冲 电流 实现 电动 机 的 连续 转动 ， 改 变 脉冲 的 频率 即 可 改变 电动 机 的 转速 ， 改 变 施 加 脉冲 的 顺序 即 可 改变 电动 机 的 转动 方向 ， 实 现 机 器 人 的 
位 置 控制 。 而 伺服 电动 机 是 一 个 连续 转动 的 直流 电机 ， 通 过 闭环 反馈 控制 回路 ， 以 实现 精确 的 位 置 控制 。 


. 新 型 驱 动 器 可 实现 优良 的 特色 驱动 特性 : 静电 驱动 器 可 利用 电荷 间 引 力 和 排斥 力 的 互相 作用 顺序 驱动 电极 而 产生 平移 或 旋转 ;形状 记忆 合金 驱动 器 由 于 具有 形状 记忆 效应 ， 即 力作 用 后 会 发 生 弹 性 变 
形 ; 压 电 效应 驱动 器 在 外 力作 用 下 不 仅 几何 尺寸 会 发 生变 化 ， 而 且 内 部 也 会 出 现 极 化 ， 当 外 力 消失 时 ， 材 料 重新 恢复 到 原来 的 状态 ， 电 场 也 随即 消失 ， 因 此 称 为 压 电 效 应 ; 为 了 更 好 地 模拟 生物 体 的 运动 功 
能 以 应 用 于 智能 机 器 人 ， 目 前 已 经 研制 出 了 各 种 不 同类 型 的 人 工 肌肉。 


(4) 控制 计算 机 
控制 计算 机 可 实现 智能 机 器 人 的 智能 感知 、 认 知 理解 、 决 策 控 制 、 任 务 协 同等 处 理 与 管理 任务 。 其 发 展 具 有 以 下 趋势 : 


“ 从 通用 到 专用 ， 从 联机 到 谱 入 : 采用 通用 计算 机 作为 机 器 人 的 计算 与 控制 - 与 机 器 人 主体 通过 专线 或 网 络 在 线 连接 ， 这 是 早期 的 纯 软 件 解决 方案 ， 其 具有 系统 开发 的 方便 性 和 功能 实现 的 灵活 
性 ， 但 其 难以 支持 智能 机 器 人 的 集成 性 、 移 动 性 及 微型 化 等 需求 。 因 此 ， 采用 诬 入 式 计算 机 ， 配 置 专用 处 理 硬件 的 软 硬 件 结 合 解决 方案 成 为 智能 机 器 人 控制 计算 机 的 主流 方案 。 例如， 采用 大 规模 集成 电路 
芯片 微 控制 器 (MCU) 作为 智能 机 器 人 控制 计算 机 ， 其 内 部 集成 中 央 处 理 器 单元 (CPU) 、 各 种 存储 器 (RAM、ROM、EPROM、EEPROM 和 Flash ROM) 、 多 种 输入 /输出 接口 等 众多 功能 部 件 ， 一 片 芯 片 
构成 了 一 个 基本 的 能 入 式微 型 计算 系统 。 例 如 ， 意 法 半导体 (GT) 公 可 研发 的 微 控制 器 STM32F7 已 在 新 一 代 智 能 机 器 人 系统 中 得 到 了 应 用 。 


` 从 单 处 理 器 到 多 处 理 器 ， 从 集中 结构 到 分 布 结构 : 随 着 智能 机 器 人 智能 程度 的 不 断 提高 ， 其 处 理 系统 对 实时 性 、 数 据 量 和 计算 要 求 的 不 断 提高 ， 采 用 高 性 能 谱 入 式 处 理 器 与 高 速 DSP 处 理 器 、 单 片 微 
控制 器 结合 组 成 的 异 构 多 处 理 器 成 为 智能 机 器 人 处 理 平台 的 主要 方式 ; 随 着 智能 传感器 的 发 展 ， 大 型 智能 工业 机 器 人 的 功能 与 结构 更 加 复杂 ， 集 中 处 理 方 式 已 经 难以 满足 功能 扩展 、 结 构 演化 的 需求 ， 基 于 
机 器 人 内 部 网 络 的 分 布 式 处 理 成 为 一 种 新 型 


(5) 人 机 交互 与 网 络 通 信 接 口 
为 了 实现 智能 机 器 人 与 操作 人 员 的 交互 与 协同 ， 以 及 与 其 他 智能 机 器 人 的 通信 与 协作 ， 智 能 机 器 人 必须 配置 多 样 化 的 人 机 交互 与 网 络 通信 接口 。 


人 机 交互 多 样 化 : 机 器 人 的 人 机 交互 实现 了 操作 人 员 参 与 智能 机 器 人 控制 并 与 机 器 人 交流 。 最 早 的 机 器 人 人 机 交互 的 方式 是 通过 鼠标 、 键 盘 等 接触 式 接口 实现 的 ， 交 互 方式 呈 机 械 化 。 随 着 传感器 技 
术 的 发 展 ， 机 器 人 人 机 交互 的 方式 体现 出 了 多 样 化 。 基 于 视觉 或 语音 的 人 机 交互 方式 是 主流 交互 方式 之 一 ， 但 其 感知 范围 和 操作 员 的 临场 感受 到 了 限制 ; 基于 穿戴 式 设 备 的 交互 方式 可 以 大 大 增强 操作 员 的 
临场 感 ， 但 价格 昂贵 ， 以 及 对 人 类 肢体 的 束缚 使 其 目前 还 难以 大 范围 推广 ; 采用 深度 摄像 机 获取 人 手 三 维 位 置 数据 、 采 用 惯性 测量 单元 获取 人 手 三 维 姿态 数据 形成 混合 传感器 的 交互 方法 ， 可 实现 人 与 机 器 
人 之 间 基 于 手势 的 自然 交互 ; 为 了 突破 传统 机 器 人 控制 的 专用 复杂 性 与 封闭 性 ， 有 研究 者 创新 地 将 iPhone 等 智能 手机 和 先进 的 交互 技术 (如 MULTI-TOUCH、 加 速 计 等 ) 与 机 器 人 技术 有 机 融合 ， 实 现 了 具有 
基于 HMM 和 人 体 动作 控制 解析 能 力 的 高 易 用 性 人 机 接口 ; 诞生 于 20 世 纪 的 虚拟 现实 眼镜 基于 眼球 追踪 技术 ， 具 有 良好 的 沉浸 性 和 实时 交互 性 ， 已 成 为 聊天 等 智能 服务 机 器 人 的 主流 人 机 交互 方式 ; 利用 4G 技 
术 接 入 Internet 构 建 具有 实时 网 络 视频 反馈 的 远程 智能 机 器 人 控制 平台 ， 可 形成 控制 灵活 、 无 距离 限制 的 高 友好 度 的 机 器 人 人 机 交互 模式 ;未 来 的 智能 机 器 人 人 机 交互 是 一 种 包括 语音 、 肢 体 动作 、 面 部 特征 


等 综合 的 自然 人 机 交互 方式 。 


.多 机 协同 网 络 化 : 在 工业 、 军 事 等 应 用 领域 ， 往 往 需要 发 挥 多 个 智能 机 器 人 的 功能 分 布 及 时 空 分 布 等 特点 ， 多 机 器 人 协同 操作 ， 共 同 完成 预定 的 复杂 任务 ， 这 就 需要 相应 的 通信 网 络 对 其 提供 支持 。 
基于 有 线 局 域 网 和 无 线 局 域 网 通信 技术 及 其 应 用 已 经 普及 ， 多 智能 机 器 人 通信 网 络 大 多 在 有 线 局 域 网 或 无 线 局 域 网 的 基础 上 依据 多 机 器 人 组 成 结构 和 应 用 场景 等 特点 进行 扩展 和 优化 实现 。 例 如 ， 多 移动 机 
器 人 采用 Ad hoc 网 络 ， 依 据 多 机 器 人 的 主 从 结构 采用 C/S 通 信 控 制 ， 为 了 组 网 的 方便 性 和 增强 抗 毁 性 采用 点 对 点 的 对 等 通信 模式 等 。 需 要 优化 之 处 主要 是 提高 网 络 通信 的 实时 性 和 可 靠 性、 网 络 组 织 的 灵活 性 
和 快捷 性 。 


1.2 智能 机 器 人 的 组 成 


1.2.1 智能 机 器 人 的 硬件 组 成 


智能 机 器 人 所 具备 的 感知 、 规 划 、 动 作 和 协同 等 能 力 确 定 了 其 硬件 的 组 成 与 结构 。 为 了 感知 和 识别 对 象 和 环境 ， 其 需要 丰富 的 内 部 传感器 和 外 部 传感器 ; 为 了 作用 于 对 象 并 影响 周围 环境 ， 其 需要 多 种 
能 力 的 动作 执行 器 ; 智能 机 器 人 的 核心 是 一 个 相当 发 达 的 “大 脑 ” 一 一 控制 计算 机 ， 其 用 于 实现 机 器 人 的 实时 管控 和 智能 处 理 ; 智能 机 器 人 具有 人 机 协同 、 多 机 器 人 协作 等 能 力 ， 因 此 需要 配置 多 样 化 交互 
接口 和 网 络 通信 接口 。 综 上 所 述 其 硬件 组 成 如 图 1-1 所 示 。 


(1) 内 部 传感器 组 
完成 机 器 人 自身 位 置 、 速 度 、 力 度 等 实时 测量 和 感知 ， 为 实现 伺服 控制 提供 依据 。 


ALE (位 移 ) 传感器 : 感知 机 器 人 当前 位 置 和 位 移 状 态 ， 其 又 可 分 为 直线 移动 传感器 和 角 位 移 传 感 器 ; 例如 ， 实 现 角 位 移 感知 的 光电 编码 器 能 够 得 到 对 应 于 编码 器 初始 锁定 位 置 的 驱动 轴 了 瞬时 角度 
值 。 当 设备 感受 到 压力 时 ， 只 要 读 出 每 个 关节 编码 器 的 读数 ， 就 能 及 时 调整 伺服 控制 的 给 定 值 ， 以 防止 机 器 人 局 动 时 产生 过 于 剧烈 的 运动 。 





图 1-1 等 能 机 器 人 的 硬件 组 成 与 基本 结构 


- 速度 和 加 速度 传感器 : 主要 用 于 感知 旋转 运动 的 速度 ， 常 用 光电 脉冲 式 转速 传感器 来 实现 ; 加 速度 传感器 可 用 于 测量 机 器 人 的 动态 控制 信号 ， 一 般 由 速度 测量 进行 推演 。 主 流 的 传感器 有 应 力 传感器 
和 振动 式 加 速度 传感器 。 


AREER: 主要 用 于 测量 两 个 物体 之 间 的 作用 力 分 量 和 力 给 分 量 。 主 要 有 半导体 型 力 觉 传感器 、 转 和 撼 传 感 器 及 基于 纺 振 动 原理 的 力 觉 传感器 等 。 例 如 ， 斯 坦 福 研 究 所 研制 的 光 腕 力 传感器 是 由 6 个 小 
型 差 动 变压器 组 成 的 ， 能 够 测量 作用 于 腕 部 X、Y 和 ZZ 三 个 方向 的 动力 及 各 轴 动 转 矩 。 


(2) 外 部 传感器 组 

通过 视觉 、 触 觉 及 声 觉 等 的 感知 ， 实 现 智 能 机 器 人 及 时 反映 环境 变化 、 自 主 调整 自身 行为 的 能 

* 视觉 传感器 : 机 器 视觉 已 成 为 智能 机 器 人 的 主要 感知 手段 ， 微 型 化 智能 数字 摄像 机 是 其 主要 选择 。 

+ 触觉 传感器 : 用 于 感知 动作 执行 器 与 对 象 的 接触 及 其 程度 ; 微型 开关 是 触觉 传感器 的 常用 选择 ， 隔 离 式 双 态 接触 传感器 、 算 阵 传感器 、 光 反射 触觉 传感器 等 也 是 新 发 展 的 触觉 传感器 。 


* 声 党 传感器 : 用 于 感受 和 解释 气体 ( 非 接触 感受 ) 、 液 体 或 固体 (接触 感受 ) 中 的 声波 。 声 波 传 感 器 可 以 从 简单 的 声波 存在 检测 到 复杂 的 声波 频率 分 析 ， 直 到 对 连续 自然 语言 中 单独 语音 和 词汇 的 辨 


` 滑 党 传感器 : 用 于 检测 物体 的 滑动 。 当 要 求 智能 机 器 人 抓 住 特 性 未 知 的 物体 时 ， 必 须 确 定 最 适当 的 握力 值 ， 所 以 要 事先 检测 出 握力 不 够 时 所 产生 的 物体 滑动 信号 。 目 前 已 有 基于 光学 和 利用 晶体 接收 
器 等 不 同 原理 的 滑 觉 传感器 ， 后 者 的 检测 灵敏 度 与 滑动 方向 无 关 。 


- 其 他 外 传感器 : 用 于 智能 移动 机 器 人 的 距离 感知 的 激光 测 距 仪 、 声 呐 传感器 等 距离 传感器 ; 用 于 感知 环境 温度 的 接触 式 或 非 接触 式 温度 传感器 ; 用 于 检测 物体 的 存在 和 测量 距离 的 超声 波 接近 度 传 感 


(3) 驱动 与 作 动 器 
驱动 与 作 动 器 是 驱动 智能 机 器 人 活动 的 动力 机 构 。 驱 动 器 可 将 电能 、 液 压 能 和 气压 能 转化 为 机 器 人 的 动力 ， 并 驱动 自身 移动 或 机 械 辟 关节 的 移动 。 


` 驱动 电动 机 是 智能 机 器 人 的 基本 驱动 器 : 机 器 人 驱动 器 的 本 质问 题 是 控制 电动 机 ， 控 制 电动 机 的 转 数 就 可 以 控制 机 器 人 移动 的 距离 和 方向 、 机 械 展 的 谊 曲 度 或 移动 距离 等 ， 驱 动 电 动机 主要 有 步 进 电 
动机 和 伺服 电动 机 。 步 进 电 动机 是 通过 给 内 部 多 个 线圈 依次 施加 脉冲 电流 实现 电动 机 的 连续 转动 ， 改 变 脉冲 的 频率 即 可 改变 电动 机 的 转速 ， 改 变 施 加 脉冲 的 顺序 即 可 改变 电动 机 的 转动 方向 ， 实 现 机 器 人 的 
位 置 控 制 。 而 伺服 电动 机 是 一 个 连续 转动 的 直流 电机 ， 通 过 闭环 反馈 控制 回路 ， 以 实现 精确 的 位 置 控制 。 


` 新 型 驱动 器 可 实现 优良 的 特色 驱动 特性 : 静电 驱动 器 可 利用 电荷 间 引 力 和 排斥 力 的 互相 作用 顺序 驱动 电极 而 产生 平移 或 旋转 ; 形状 记忆 合金 驱动 器 由 于 具有 形状 记忆 效应 ， 即 力作 用 后 会 发 生 弹 性 变 
形 ; 压 电 效应 驱动 器 在 外 力作 用 下 不 仅 几 何 尺 寸 会 发 生变 化 ， 而 且 内 部 也 会 出 现 极 化 ， 当 外 力 消失 时 ， 材 料 重新 恢复 到 原来 的 状态 ， 电 场 也 随即 消失 ， 因 此 称 为 压 电 效应 ; 为 了 更 好 地 模拟 生物 体 的 运动 功 
能 以 应 用 于 智能 机 器 人 ， 目 前 已 经 研制 出 了 各 种 不 同类 型 的 人 工 肌 肉 。 


(4) 控制 计算 机 
控制 计算 机 可 实现 智能 机 器 人 的 智能 感知 、 认 知 理解 、 决 策 控 制 、 任 务 协 同等 处 理 与 管理 任务 。 其 发 展 具 有 以 下 趋势 : 


. 从 通用 到 专用 ， 从 联机 到 谋 入 : 采用 通用 计算 机 作为 机 器 人 的 计算 与 控制 平台 ， 与 机 器 人 主体 通过 专线 或 网 络 在 线 连接 ， 这 是 早期 的 纯 软 件 解决 方案 ， 其 具有 系统 开发 的 方便 性 和 功能 实现 的 灵活 
性 ， 但 其 难以 支持 智能 机 器 人 的 集成 性 、 移 动 性 及 微型 化 等 需求 。 因 此 ， 采 用 吝 入 式 计算 机 ， 配 置 专用 处 理 硬件 的 软 硬 件 结合 解决 方案 成 为 智能 机 器 人 控制 计算 机 的 主流 方案 。 例如， 采用 大 规模 集成 电路 
芯片 微 控制 器 (MCU) 作为 智能 机 器 人 控制 计算 机 ， 其 内 部 集成 中 央 处 理 器 单元 (CPU) 、 各 种 存储 器 (RAM、ROM、EPROM、EEPROM 和 Flash ROM) 、 多 种 输入 /输出 接口 等 众多 功能 部 件 ， 一 片 芯片 
构成 了 一 个 基本 的 嵌入 式微 型 计算 系统 。 例 如 ， 意 法 半导体 (GT) 公 可 研发 的 微 控 制 器 STM32F7 已 在 新 一 代 智 能 机 器 人 系统 中 得 到 了 应 用 。 


" 从 单 处 理 器 到 多 处 理 器 ， 从 集中 结构 到 分 布 结构 : 随 着 智能 机 器 人 智能 程度 的 不 断 提高 ， 其 处 理 系统 对 实时 性 、 数 据 量 和 计算 要 求 的 不 断 提高 ， 采 用 高 性 能 谋 入 式 处 理 器 与 高 速 DSP 处 理 器 、 单 片 微 


控制 器 结合 组 成 的 异 构 多 处 理 器 成 为 智能 机 器 人 处 理 平台 的 主要 方式 ; 随 着 智能 传感器 的 发 展 ， 大 型 智能 工业 机 器 人 的 功能 与 结构 更 加 复杂 ， 集 中 处 理 方式 已 经 难以 满足 功能 扩展 、 结 构 演 化 的 需求 ， 基 于 
机 器 人 内 部 网 络 的 分 布 式 处 理 成 为 一 种 新 型 


(5) 人 机 交互 与 网 络 通 信 接 口 
为 了 实现 智能 机 器 人 与 操作 人 员 的 交互 与 协同 ， 以 及 与 其 他 智能 机 器 人 的 通信 与 协作 ， 智 能 机 器 人 必须 配置 多 样 化 的 人 机 交互 与 网 络 通信 接口 。 


.人 机 交互 多 样 化 : 机 器 人 的 人 机 交互 实现 了 操作 人 员 参 与 智能 机 器 人 控制 并 与 机 器 人 交流 。 最 早 的 机 器 人 人 机 交互 的 方式 是 通过 鼠标、 键盘 等 接触 式 接口 实现 的 ， 交 互 方式 呈 机 械 化 。 随 着 传感器 技 
本 的 发 展 ， 机 器 人 人 机 交互 的 方式 体现 出 了 多 样 化 。 基 于 视 党 或 语音 的 人 机 交互 方式 是 主流 交互 方式 之 一 ， 但 其 感知 范围 和 操作 员 的 临场 感受 到 了 限制 ; 基于 穿戴 式 设备 的 交互 方式 可 以 大 大 增强 操作 员 的 
临场 感 ， 但 价格 昂贵 ， 以 及 对 人 类 肢体 的 束缚 使 其 目前 还 难以 大 范围 推广 ; 采用 深度 摄像 机 获取 人 手 三 维 位 置 数 据 、 采 用 惯性 测量 单元 获取 人 手 三 维 姿 态 数 据 形 成 混合 传感器 的 交互 方法 ， 可 实现 人 与 机 器 
人 之 间 基 于 手势 的 自然 交互 ;为 了 突破 传统 机 器 人 控制 的 专用 复杂 性 与 封闭 性 ， 有 研究 者 创新 地 将 iPhone 等 智能 手机 和 先进 的 交互 技术 (如 MULTI-TOUCH、 加 速 计 等 ) 与 机 器 人 技术 有 机 融合 ， 实 现 了 具有 
基于 HMM 和 人 体 动作 控制 解析 能 力 的 高 易 用 性 人 机 接口 ; 诞生 于 20 世 纪 的 虚拟 现实 眼镜 基于 眼球 追踪 技术 ， 具 有 良好 的 沉浸 性 和 实时 交互 性 ， 已 成 为 聊天 等 智能 服务 机 器 人 的 主流 人 机 交互 方式 ; 利用 4G 技 
术 接 入 Intetnet 构 建 具有 实时 网 络 视频 反馈 的 远程 智能 机 器 人 控制 平台 ， 可 形成 控制 灵活 、 无 距离 限制 的 高 友好 度 的 机 器 人 人 机 交互 模式 ; 未 来 的 智能 机 器 人 人 机 交互 是 一 种 包括 语音 、 肢 体 动作 、 面 部 特征 
等 综合 的 自然 人 机 交互 方式 。 


.多 机 协同 网 络 化 : 在 工业 、 军 事 等 应 用 领域 ， 往 往 需 要 发 挥 能 机 器 人 的 功能 分 布 及 时 空 分 布 等 特点 ， 多 机 器 人 协同 操作 ， 共 同 完成 预定 的 复杂 任务 ， 这 就 需要 相应 的 通信 网 络 对 其 提供 支持 。 
基于 有 线 局 域 网 和 无 线 局 域 网 通信 技术 及 其 应 用 已 经 普及 ， 多 智能 机 器 人 通信 网 络 大 多 在 有 线 局 域 网 或 无 线 局 域 网 的 基础 上 依据 多 机 器 人 组 成 结构 和 应 用 场景 等 特点 进行 扩展 和 优化 实现 。 例 如 ， 多 移动 机 
器 人 采用 Ad hoc 网 络 ， 依 据 多 机 器 人 的 主 从 结构 采用 C/S 通 信 控 制 ， 为 了 组 网 的 方便 性 和 增强 抗 毁 性 采用 点 对 点 的 对 等 通信 模式 等 。 需 要 优化 之 处 主要 是 提高 网 络 通信 的 实时 性 和 可 靠 性 、 网 络 组 织 的 灵活 性 
和 快捷 性 。 


1.2.2 智能 机 器 人 的 软件 系统 

除了 上 述 智能 机 器 人 的 机 械 本 体 和 硬件 组 成 之 外 ， 软 件 系 统 也 是 智能 机 器 人 的 重要 组 成 部 分 。 其 运行 在 智能 机 器 人 特定 的 硬件 平台 之 上 ， 不仅 实现 了 机 器 人 运动 位 置 控制 、 姿 态 轨 迹 计 划 、 操 作 顺 序 管 
理 、 人 机 友好 交互 及 多 机 通信 与 协同 等 功能 ， 而 且 支 持 智能 机 器 人 软件 与 系统 的 仿真 、 开 发 、 测 试 与 验证 等 环节 。 

1. 智 能 机 器 人 的 软件 系统 结构 


智能 机 器 人 的 软件 系统 一 般 分 为 运行 时 支持 系统 和 仿真 开发 平台 两 大 部 分 。 如 图 1-2 所 示 。 
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图 1-2 智能 机 器 人 的 软件 系统 





J 时 支持 系统 驻 留 在 智能 机 器 人 的 处 理 机 平台 之 上 ， 通 常 采 用 层次 化 结构 。 自 底 向 上 的 第 一 层 是 开源 多 任务 操作 系统 Linux 或 专用 嵌入 式 实 时 操作 系统 ， 其 除了 实现 基本 的 处 理 资 源 管 理 、 多 任务 调度 


之 外 ， 还 实现 了 与 智能 机 器 人 硬件 驱动 的 适 配 ; 第 二 层 是 核心 通用 构件 层 ， 实 现 运 动 控制 、 坐 标 变换 、 人 机 交互 等 通用 功能 ;第 三 层 是 特定 应 用 构件 层 ， 其 实现 图 像 处 理 、 路 径 规划 、 定 位 导航 等 应 用 支持 
功能 。 三 个 层次 分 别 面 对 不 同 的 功能 需求 ， 对 应 于 不 同 层次 的 开发 ， 系 统 中 各 个 层次 内 部 均 是 由 若干 个 功能 相对 应 的 系统 构件 组 成 的 ， 这 些 功能 构件 相互 协作 共同 实现 该 层次 所 提供 的 功能 。 


智能 机 器 人 仿真 开发 平台 可 以 在 宿主 机 上 运行 ， 主 要 支持 实现 智能 机 器 人 运行 软件 的 高 效 开 发 。 不 仅 支 持 智能 机 器 人 的 模型 构建 ， 而 且 具 有 驱动 描述 、 编 程 语言 支持 等 能 力 ; 不 仅 支 持 功能 设计 验证 ， 


而 且 具 有 运行 环境 仿真 、 动 态 可 视 演示 等 能 力 。 由 此 可 知 ， 智 能 机 器 人 仿真 开发 平台 是 其 软件 系统 必 不 可 少 的 一 部 分 ， 并 且 具 有 面向 特定 领域 的 适应 性 和 智能 化 软件 应 用 开发 的 高 效 性 等 特点 。 
2. 智 能 机 器 人 操作 系统 


如 前 所 述 ， 可 以 将 智能 机 器 人 看 作 一 个 特殊 用 途 的 专用 嵌入 式 控制 系统 ， 犹 如 智能 手机 作为 移动 终端 专用 嵌入 式 系统 ， 配 置 安 卓 等 操作 系统 后 ， 不 仅 使 用 方便 ， 而 且 有 利于 高 效应 用 开发 ， 更 可 通过 
APP 下 载 无 限 扩展 其 能 力 ， 智 能 机 器 人 也 可 配置 相应 的 操作 系统 。 智 能 机 器 人 操作 系统 的 主要 功能 是 屏 菩 异 构 的 机 器 人 硬件 资源 ， 实 现实 时 多 任务 调度 与 分 布 式 实 时 通信 、 支 持 多 模式 人 机 交互 ， 并 支持 智 
能 机 器 人 应 用 软件 的 高 效 开 发 ， 从 而 有 效 管理 机 器 人 硬件 与 软件 资源 。 智 能 机 器 人 操作 系统 的 实现 存在 多 种 途径 ， 主 流 途 径 是 基于 已 有 的 Linux、Windows 等 操作 系统 内 核 ， 依 据 智 能 机 器 人 的 特点 与 需求 
进行 优化 和 扩展 ， 也 有 完全 设计 适应 智能 机 器 人 特点 的 全 新 操作 系统 。 需 要 说 明 的 是 ， 有 的 虽 称 为 智能 机 器 人 操作 系统 ， 但 其 本 质 已 超越 了 传统 计算 机 操作 系统 的 职能 ， 以 智能 机 器 人 中 间 件 和 开发 环境 为 
主体 。 


3. 智 能 机 器 人 软件 构件 化 


构件 化 是 一 种 提高 机 器 人 软件 重用 性 和 降低 系统 设计 难度 的 主流 方法 。 实 现 机 器 人 软件 系统 构件 化 的 前 提 是 机 器 人 系统 从 功能 上 被 分 解 成 了 各 种 可 重用 的 构件 。 为 此 需要 建立 构件 的 静态 模型 和 运行 时 
模型 ， 并 设计 基于 模型 的 构件 接口 定义 语言 ， 不 同 构件 之 间 的 通信 可 通过 相应 的 中 间 件 来 实现 ， 最 终 通 过 构件 之 间 的 聚集 和 整合 高 效 地 形成 完整 的 机 器 人 软件 系统 。 


首先 ， 机 器 人 系统 被 分 成 控制 系统 和 被 控 系 统 。 控 制 系统 是 机 器 人 控制 软件 或 者 算法 ， 被 控 系 统 是 机 器 人 硬件 。 为 了 实现 机 器 人 系统 的 构件 化 设计 和 软件 的 重用 ， 可 采用 基于 构件 的 抽象 模型 以 解除 控 
制 系统 和 被 控 系 统 之 间 的 紧 耦 合 。 如 图 1-3a 所 示 ， 机 器 人 中 间 件 把 机 器 人 系统 分 成 软件 系统 和 硬件 系统 两 个 部 分 ， 机 器 人 中 间 件 的 下 层 是 机 器 人 硬件 系统 ， 由 不 同 的 传感器 、 执 行 器 和 其 他 硬件 设备 组 成 。 
中 间 件 的 上 层 是 机 器 人 控制 软件 ， 由 不 同 的 机 器 人 软件 构件 、 控 制 算 法 和 服务 组 成 。 机 器 人 中 间 件 隐藏 了 底层 硬件 的 异 构 性 ， 同 时 还 提供 了 独立 于 机 器 人 硬件 系统 的 构件 接口 ， 这 些 接口 可 用 于 上 层 构 件 或 
具体 应 用 。 为 了 提高 软件 的 可 重用 性 ， 机 器 人 软件 系统 被 分 解 成 一 系列 的 不 同类 型 和 不 同 粒度 的 可 重用 软 构件 。 


在 图 1-3b 中 ， 水 平方 向 的 机 器 人 系统 被 分 成 不 同 的 功能 构件 。 这 些 不 同 功能 构件 的 设计 开发 者 能 够 专注 于 他 们 各 自 的 功能 实现 ， 机 器 人 系统 集成 者 能 够 根据 标准 接口 快速 地 集成 不 同 的 功能 构件 并 开发 
机 器 人 软件 系统 ， 而 不 需要 理解 功能 构件 特定 的 实现 细节 。 
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图 1-3 ”构件 化 机 器 人 软件 系统 架构 

在 垂直 方向 存在 4 种 不 同 程度 细 粒 度 的 构件 ， 这 些 都 是 基于 不 同 程度 的 抽象 和 重用 性 ， 具 体 如 下 。 

“ 原子 构件 : 原子 构件 适合 于 硬件 组 件 有 直接 联系 的 唯一 构件 ， 其 是 一 种 特殊 的 硬件 抽象 ， 并 且 提 供 了 对 高 层 构件 调用 的 统一 接口 ， 如 电动 机 控制 原子 构件 、 传 感 器 控制 原子 构件 等 。 
“ 复合 构件 : 根据 一 些 功能 需求 和 组 合 规则 ， 一 些 原子 构件 被 组 合 在 一 起 形成 复合 构件 ， 这 些 构件 提供 了 更 高 层次 和 更 大 粒度 的 构件 抽象 。 

" 算法 构件 : 算法 构件 是 一 种 与 平台 独立 的 机 器 人 算法 实现 ， 或 者 是 一 般 机 器 人 控制 算法 ， 如 路 径 规划 、 卡 尔 曼 滤 波 等 。 

“ 应 用 构件 : 应 用 构件 包括 原子 构件 、 复 合 构件 和 算法 构件 各 种 类 型 构件 的 组 合 ， 用 于 实现 机 器 人 系统 的 应 用 功能 。 

4 .智能 机 器 人 开源 共享 软件 


目前 世界 上 各 大 工业 机 器 人 厂商 均 开发 了 各 自 的 机 器 人 软件 系统 。 这 些 软 件 系 统 往往 是 私有 且 封 闭 的， 厂商 只 提供 了 固定 的 操作 方式 和 十 分 有 限 的 编程 接口 供用 户 使 用 。 对 于 机 器 人 研发 者 来 说 ， 如 果 
购买 这 些 厂 商 的 机 器 人 进行 进一步 研发 ， 会 受到 很 多 限制 ， 而 自行 开发 从 底层 到 上 层 的 完整 软件 系统 ， 又 会 成 本 过 高 并 且 不 现实 。 


另外 ， 虽 然 不 同 的 机 器 人 和 不 同 的 应 用 环境 对 机 器 人 软件 系统 的 要 求 是 不 同 的 ， 但 是 有 不 少 功能 是 每 个 开发 者 都 要 面 对 的 共性 问题 ， 比 如 坐标 变换 、 模 块 间 通 信 等 。 如 果 每 个 研发 者 都 要 自行 开发 这 些 
共性 功能 ， 不 仅 浪 费时 间 ， 分 散 研 发 者 的 资源 ， 并 且 使 研发 者 无 法 充分 投入 到 更 有 意义 及 价值 的 目标 研发 中 。 


针对 这 些 问题 的 解决 需求 ， 开 放 源 代码 的 机 器 人 操作 系统 (RobotOperating System, ROS) 应 运 而 生 。2010 年 Willow Garage 公 司 发 布 了 开源 机 器 人 操作 系统 ROS， 很 快 在 机 器 人 研究 领域 展开 了 学 
习 和 使 用 ROS 的 热潮 。 其 宗旨 在 于 构建 一 个 能 够 整合 不 同 研 究 成 果 ， 实 现 算 法 发 布 、 代 码 重用 的 机 器 人 软件 平台 。ROS 中 实现 了 大 量 的 机 器 人 技术 中 的 共性 问题 ， 这 使 得 ROS 的 使 用 者 可 以 充分 复 用 ROS 中 
已 有 的 高 质量 代码 ， 而 不 必 自 己 重 复 开 发 。 


1.3 ”智能 机 器 人 的 分 类 


智能 机 器 人 依据 其 活动 方式 可 分 为 固定 型 和 移动 型 两 大 类 。 前 者 是 其 整体 位 置 固定 ， 主 要 是 靠 机 器 人 和 辟 自主 活动 ， 自 动 完成 指定 任务 ;而 后 者 是 机 器 人 整体 可 依据 任务 需求 自主 活动 。 更 多 情况 下 则 是 
依据 智能 机 器 人 的 用 途 进行 分 类 ， 因 为 不 同 用 途 的 智能 机 器 人 ， 其 组 成 结构 、 实 现 功能 、 活 动 形态 等 都 具有 较 大 差别 。 据 此 ， 一 般 可 将 其 分 为 工业 智能 机 器 人 、 农 业 智 能 机 器 人 、 探 索 智能 机 器 人 、 服 务 智 
能 机 器 人 等 。 


(1) 工业 智能 机 器 人 


工业 智能 机 器 人 依据 具体 应 用 的 不 同 ， 通 常 又 可 以 细 分 为 焊接 机 器 人 、 装 配 机 器 人 、 了 喷漆 机 器 人 、 码 埃 机器人、 搬运 机 器 人 等 多 种 类 型 。 作 为 具有 智能 的 工业 机 器 人 ， 它 们 在 很 多 方面 已 经 超越 了 传统 
机 器 人 。 焊 接 机 器 人 包括 点 焊 和 电弧 焊 机 器 人 ， 其 用 途 是 实现 自动 的 焊接 作业 。 装 配 机 器 人 比较 多 地 用 于 电子 部 件 及 电器 等 装配 。 喷 漆 机 器 人 代 蔡 人 进行 喷漆 作业 。 码 志 、 上 下 料 、 搬 运 机 器 人 的 功能 则 是 
根据 一 定 的 速度 和 精度 要 求 ， 将 物品 从 一 处 搬运 到 另 一 处 。 在 工业 生产 中 应 用 各 种 机 器 人 ， 可 以 方便 迅速 地 改变 作业 的 内 容 或 方式 ， 以 适应 生产 要 求 的 变化 。 例 如 ， 改 变 焊 缝 轨迹、 改变 喷漆 位 置 、 变 更 装 
配 部 件 或 位 置 ， 等 等 。 随 着 对 工业 生产 线 柔性 的 要 求 越 来 越 高 ， 对 各 种 机 器 人 的 需求 也 就 越 来 越 强烈 。 


(2) 农业 智能 机 器 人 


随 着 机 器 人 技术 的 进步 ， 以 定型 物 、 无 机 物 为 作业 对 象 的 工业 智能 机 器 人 正在 向 更 高 层次 的 以 动 、 植 物 之 类 复杂 作业 对 象 为 目标 的 农业 机 器 人 发 展 ， 农 业 智能 机 器 人 或 机 器 人 化 的 农业 机 械 的 应 用 范围 
正在 逐步 扩大 。 农 业 智能 机 器 人 的 应 用 不 仅 能 够 大 大 减轻 甚至 代替 人 们 生产 劳动 ， 从 而 解决 劳动 力 不 足 的 问题 ， 而 且 可 以 提高 劳动 生产 率 ， 改 善 农业 的 生产 环境 ， 防 止 农药 、 化 肥 等 对 人 体 的 伤害 ， 提 高 作 
业 质 量 。 但 是 由 于 农业 智能 机 器 人 所 面临 的 是 非 结构 、 不 确定 、 不 宜 预 估 的 复杂 环境 和 工作 对 象 ， 所 以 与 工业 智能 机 器 人 相 比 ， 其 研究 开发 的 难度 更 大 。 农 业 智能 机 器 人 的 研究 开发 目前 主要 集中 于 耕种 、 
施肥 、 喷 药 、 蔬 菜 嫁接 、 株 苗 移 栽 、 收 获 、 灌 没 、 养 殖 和 各 种 辅助 操作 等 方面 。 日 本 是 机 器 人 普及 最 广泛 的 国家 ， 目 前 已 经 有 多 种 智能 机 器 人 应 用 于 农业 领域 。 


(3) 探索 智能 机 器 人 


机 器 人 除了 在 工农 业 上 得 到 广泛 应 用 之 外 ， 还 越 来 越 多 地 应 用 于 极限 探索 ， 即 在 恶劣 或 不 适 于 人 类 工作 的 环境 中 执行 任务 。 例 如 ， 在 水 下 (海洋 ) 、 太 空 及 在 放射 性 、 有 毒 或 高 温 等 环境 中 进行 作业 。 
人 类 借助 潜水 器 具 潜 入 深海 之 中 探秘 已 有 很 长 的 历史 ， 然 而 由 于 危险 很 大 、 费 用 极 高 ， 所 以 水 下 机 器 人 就 成 为 代替 人 在 这 一 危险 环境 中 工作 的 最 佳 工具 。 空 间 机 器 人 是 指 在 大 气 层 内 和 大 气 层 外 从 事 各 种 作 
业 的 机 器 人 ， 包 括 在 内 层 空 间 飞行 并 进行 观测 、 可 完成 多 种 作业 的 飞行 机 器 人 ， 到 外 层 空 间 其 他 星球 上 进行 探测 作业 的 星球 探测 机 器 人 和 在 各 种 航天 器 里 使 用 的 机 器 。 


(4) 服务 智能 机 器 人 


机 器 人 技术 不 仅 在 工农 业 生 产 、 科 学 探索 中 得 到 了 广泛 应 用 ， 而 且 已 经 逐渐 渗透 到 人 们 的 日 常生 活 领 域 ， 服 务 智 能 机 器 人 就 是 这 类 机 器 人 的 总 称 。 国 际 机 器 人 联合 会 给 服务 智能 机 器 人 的 一 个 初步 定义 
是 : 一 种 以 自主 或 半 自 主 方式 运行 ， 能 为 人 类 的 生活 、 康 复 提 供 服 务 的 机 器 人 ， 或 者 是 能 对 设备 运行 进行 维护 的 一 类 机 器 人 。 尽 管 服务 智能 机 器 人 的 起 步 较 晚 ， 但 应 用 前 景 十 分 广泛 ， 目 前 主要 应 用 人 在 清 
洁 、 护 理 、 执 勤 、 救 援 、 娱 乐 和 代替 人 对 设备 进行 维护 保养 等 场合 。 


1.3.1 服务 智能 机 器 人 


1. 服 务 智能 机 器 人 的 发 展 历程 


在 过 去 ， 服 务 智 能 机 器 人 只 是 存在 于 科幻 电影 当中 ， 发 展 到 当代 ， 服 务 智能 机 器 人 在 军事 、 服 务 和 娱乐 各 个 领域 都 发 挥 着 越 来 越 重 要 的 作用 ， 正 在 逐渐 取代 人 类 的 一 些 工 作 。 目 前 世界 上 有 近 50 个 国家 
在 发 展 服务 智能 机 器 人 ， 其 中 以 美国 、 德 国 、 法 国 、 日 本 和 韩国 为 代表 的 发 达 国 家 在 该 领域 处 于 领先 地 位 。 代 表 性 服务 智能 机 器 人 如 美国 的 面向 家 庭 养老 陪护 的 机 器 人 、 日 本 开发 的 家 务 机 器 人 ， 以 及 与 小 
孩 进行 教育 沟通 娱乐 的 机 器 人 ， 其 应 用 已 经 较为 广泛 。 我 国 在 20 世 纪 80 年 代 已 出 现 了 各 种 服务 型 机 器 人 雏形 ，90 年 代 国家 863 项 目 给 予 了 重点 支持 ， 进 入 21 世 纪 ， 尤 其 是 近年 来 ， 我 国 的 服务 智能 机 器 人 在 
应 用 需求 牵引 和 国家 支持 下 也 得 到 了 快速 帮 展 ， 如 国家 电网 的 巡 检 机 器 人 、 比 较 火 热 的 送 餐 机 器 人 等 。 服 务 智 能 机 器 人 的 市 场 化 程度 目前 处 于 起 步 阶 段 ， 但 随 着 劳动 力 不 足 及 老龄 化 的 影响 ， 未 来 应 用 会 有 
很 大 的 发 展 ， 人 们 会 在 生活 当中 遇 到 形形色色 、 各 种 各 样 的 服务 智能 机 器 人 。 从 发 展 趋势 来 看 ， 我 国 专业 服务 机 器 人 有 望 先 于 个 人 /家 用 机 器 人 实现 产业 化 ， 特 别 是 医疗 机 器 人 、 危 险 特殊 环境 巡 检 、 救 援 机 
器 人 等 。 随 着 我 国 进入 老龄 化 社会 ， 医 疗 、 护 理 和 康复 的 需求 逐渐 增加 ， 同 时 由 于 人 们 对 生活 品质 追求 的 提高 ， 个 人 /家 用 机 器 人 在 未 来 也 因此 具有 了 更 为 广阔 的 市 场 空间 。 


2. 服 务 智能 机 器 人 的 分 类 


服务 智能 机 器 人 具有 广泛 的 应 用 范围 ， 可 分 为 专用 服务 智能 机 器 人 和 家 用 服务 智能 机 器 人 两 大 类 。 前 者 指 康 复 、 助 残 机 器 人 ， 危 险 特殊 环境 巡 检 、 救 援 机 器 人 等 具有 专业 用 途 的 服务 智能 机 器 人 ， 后 者 
是 指 家 庭 环境 中 应 用 的 清洁 、 教 育 、 娱 乐 等 服务 智能 机 器 人 。 无 论 是 哪 类 服务 智能 机 器 人 ， 随 着 智能 化 水 平 的 不 断 提升 ， 服 务 智 能 机 器 人 是 其 主要 发 展 方向 。 服 务 智 能 机 器 人 的 类 别 如 图 1-4 所 示 。 
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图 1-4 服务 智能 机 器 人 分 类 
3. 人 形 服务 智能 机 器 人 


人 形 服务 智能 机 器 人 是 指 机 器 人 形态 与 人 相似 的 服务 机 器 人 ， 如 图 1-5 所 示 ， 其 发 展 可 在 未 来 真正 使 人 类 的 生活 产生 深刻 变革 。 因 为 从 人 们 对 机 器 人 的 认识 来 说 ， 类 人 形 机 器 人 在 外 观 上 与 人 更 为 相近 ， 
因此 人 们 更 易 接 受 。 所 以 未 来 真正 的 服务 智能 机 器 人 可 能 朝 着 类 人 形态 发 展 。 人 形 机 器 人 发 展 以 注重 自然 的 人 机 交互 为 重点 ， 即 不 仅 是 具有 人 的 形状 ， 更 要 与 人 进行 自然 的 沟通 和 交流 。 面 向 消费 级 用 户 来 
说 ， 高 性 价 比 和 持续 服务 能 力也 应 是 人 形 服务 机 器 人 追求 的 目标 之 一 。 从 更 接近 人 类 的 角度 研发 高 逼真 度 的 服务 智能 机 器 人 ， 其 未 来 在 迎 宾 服 务 和 娱乐 影视 方面 会 有 较 大 的 发 展 。 
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图 1-5 人形 服务 智能 机 器 人 


4 .服务 智能 机 器 人 的 关键 技术 


服务 智能 机 器 人 的 发 展 不 仅 应 重点 突破 生机 电感 知 与 融合 、 定 位 导航 与 路 径 规 划 等 通用 关键 技术 ， 而 且 应 注重 面向 不 同 用 户 、 不 同 环境 的 服务 智能 机 器 人 的 高 柔韧 性 及 安全 控制 等 专用 技术 ， 如 图 1-6 所 
示 。 作 为 未 来 与 人 类 合作 共存 的 服务 智能 机 器 人 ， 其 核心 是 为 了 感知 、 理 解 和 帮助 人 的 自然 人 机 交互 与 智能 合作 能 力 ， 这 就 需要 服务 智能 机 器 人 不 仅 具 有 视觉 交互 与 语音 识别 的 能 力 ， 还 要 能 够 自主 学 习 和 
主动 学 习 。 


除 此 之 外 ， 标 准 不 统一 是 目前 困扰 服务 机 器 人 发 展 的 主要 问题 之 一 ， 各 家 采用 的 系统 语言 、 硬 件 (核心 控制 器 、 传 感 器 、 驱 动 器 ) 和 软件 构件 不 统一 ， 没 有 形成 机 器 人 通用 的 控制 硬件 与 软件 平台 ， 缺 
乏 支 持 通 用 性 的 定制 能 力 ， 制 约 了 服务 机 器 人 的 大 规模 友 展 。 因 此 标准 化 、 平 台 化 也 是 服务 智能 机 器 人 发 展 的 关键 。 


服务 智能 机 器 人 关键 技术 
服务 智能 机 器 人 一 一 成 为 人 的 伙伴 
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B1-6 服务 智能 机 器 人 的 关键 技术 


1.3.2 ”工业 智能 机 器 人 


1. 工 业 智能 机 器 人 的 发 展 历程 


工业 机 器 人 是 先进 的 电气 自动 化 、 机 电 一 体 化 的 数字 化 装备 ， 集 机 械 、 电 子 、 控 制 、 计 算 机 、 传 感 器 等 多 学 科 高 新 技术 于 一 体 的 现代 机 器 ， 其 是 一 种 仿 人 操作 、 自 动 控 制 、 可 重复 编程 、 能 在 三 维 空间 
完成 各 种 作业 的 机 电 一 体 化 自动 化 生产 设备 ， 既 具有 人 类 对 环境 状态 的 快速 反应 和 分 析 判 断 能 力 ， 又 具有 机 器 可 长 时 间 持 续 工 作 、 精 确 度 高 、 抗 恶 务 环境 的 能 力 ， 目 前 广泛 应 用 于 工业 领域 的 各 行 各 业 ， 特 
别 适合 于 多 品种 、 变 批量 的 柔性 生产 。 它 对 稳定 产品 质量 ， 提 高 生产 效率 ， 改 善 劳动 条 件 和 产品 快速 更 新 换代 起 着 十 分 重要 的 作用 .。 


当今 工业 机 器 人 技术 正在 逐渐 向 着 具有 行走 能 力 、 多 种 感知 能 力 、 较 强 的 对 作业 环境 自 适应 能 力 的 智能 化 方向 发 展 ， 从 而 形成 工业 智能 机 器 人 。 工 业 智能 机 器 人 集 现代 制造 技术 、 新 型 材料 技术 和 信息 
控制 技术 为 一 体 ， 是 智能 制造 的 代表 性 产品 ， 对 其 的 研发 、 制 造 、 应 用 成 为 衡量 一 个 国家 科技 创新 和 制造 业 水 平 的 重要 标志 ， 引 起 了 世界 制造 强国 的 高 度 重 视 。 当 前 美国 在 工业 智能 机 器 人 技术 的 综合 研究 
水 平 上 仍 处 于 领先 地 位 ， 而 日 本 生产 的 工业 智能 机 器 人 在 数量 、 种 类 方面 则 居 世 界 首位 ， 德 国 在 工业 智能 机 器 人 研发 与 应 用 方面 具有 领先 优势 。 我 国 开展 工业 机 器 人 的 研发 与 应 用 也 有 十 余年 的 历史 ， 取 得 
了 多 项 标志 性 创新 成 果 ，《 中 国 制 造 2025》 也 将 工业 智能 机 器 人 技术 自主 研发 和 工业 领域 应 用 作为 重点 之 一 。 


2. 工 业 智 能 机 器 人 的 主要 特点 
从 工业 智能 机 器 人 的 外 在 表现 和 重要 作用 出 帮 ， 其 具有 以 下 主要 特点 。 


1) 技术 先进 : 工业 智能 机 器 人 是 集 精 密 化、 柔性 化 、 智 能 化 、 软 件 应 用 开发 等 先进 制造 技术 于 一 体 ， 全 面 延 伸 人 的 体力 和 智力 的 新 一 代 生 产 工 具 ， 是 实现 生产 数字 化 、 自 动 化 、 网 络 化 及 智能 化 的 重要 
手段 ， 通 过 对 过 程 实施 检测 、 控 制 、 优 化 、 调 度 、 管 理 和 决策 ， 可 实现 增加 产量 、 提 高 质量 、 降 低 成 本 、 减 少 资源 消耗 和 环境 污染 ， 是 工业 自动 化 水 平 的 最 高 体现 。 


2) 技术 综合 性 强 : 工业 智能 机 器 人 与 自动 化 成 套 技术 集中 并 融合 了 多 个 学 科 ， 涉 及 多 项 技术 领域 ， 包 括 工 业 机 器 人 控制 技术 、 机 器 人 动力 学 及 仿真 、 机 器 人 构建 有 限 元 分 析 、 激 光 加 工 、 实 时 软件 设 
计 、 智 能 测量 、 建 模 设 计 一 体 化 、 工 厂 自动 化 及 精细 物流 等 先进 信息 与 制造 技术 ， 技 术 综合 性 强 。 


3) 应 用 领域 广泛 : 工业 智能 机 器 人 与 自动 化 成 套装 备 是 生产 过 程 的 关键 设备 ， 可 用 于 制造 、 安 装 、 检 测 、 物 流 等 生产 环节 ， 并 广泛 应 用 于 汽车 整 车 及 汽车 零 部 件 、 工 程 机 械 、 轨 道 交通 、1C 装 备 、 军 
工 、 烟 草 、 医 药 、 冶 金 及 印刷 出 版 等 众多 行业 ， 应 用 领域 非常 广泛 。 


从 工业 智能 机 器 人 内 在 特性 和 技术 实现 的 角度 分 析 ， 其 具有 以 下 主要 特点 。 


1) 高 性 能 机 电 一 体 化 : 工业 智能 机 器 人 技术 基础 是 机 械 学 和 微 电 子 学 结合 而 成 的 机 电 一 体 化 技术 ， 如 图 1-7 所 示 。 工 业 智能 机 器 人 可 达到 低 于 0.1 毫 米 的 运动 精度 ， 抓 取 重 达 一 吨 的 物体 ， 伸 展 也 可 达 三 
米 等 。 这 些 性 能 对 绝 大 部 分 的 工业 应 用 来 说 ， 足 以 圆满 完成 任务 。 随 着 机 器 人 机 电 性 能 的 逐渐 提升 ， 以 前 一 些 不 可 能 完成 的 任务 也 变 得 可 行 起 来 ， 例 如 激光 焊接 或 切割 ， 曾 需要 专门 的 高 精度 设备 来 指导 
激光 的 走向 ,但 随 着 机 器 人 精度 的 提升 ， 现 在 也 变 得 可 直接 依赖 机 器 人 本 身 的 准确 运动 来 完成 。 工 业 智能 机 器 人 具有 获取 外 部 环境 信息 的 各 种 传感器 、 具 有 记忆 及 图 像 识别 与 推理 判断 等 智能 化 功能 ， 它 们 
均 以 高 性 能 机 电 一 体 化 和 微 电 子 技术 为 基础 ， 并 与 嵌入 式 计算 机 技术 应 用 密切 相关 。 因 此 ， 工 业 智能 机 器 人 技术 的 发 展 带动 了 新 一 代 机 电 一 体 化 等 技术 的 发 展 。 
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图 1-7 工业 智能 机 器 人 的 机 电 一 体 化 


2) 人 机 合作 与 拟人 化 : 传统 的 工业 机 器 人 基于 成 本 与 技术 考虑 ， 不 会 集成 额外 的 传感器 感知 诸如 突然 月 人 触 辜 等 特殊 情况 ， 只 会 “ 傻 傻 ” 地 依据 人 类 编写 的 程序 日 复 一 日 的 动作 着 ,不 能 随机 应 变 ， 只 
适用 于 结构 化 环境 ， 完 成 重复 性 的 作业 任务 。 而 现代 机 器 人 则 希望 同人 类 一 起 在 相同 的 非 结构 化 空间 和 环境 中 协同 作业 ， 实 时 在 线 完 成 非 确定 性 的 任务 ， 所 以 工业 智能 机 器 人 应 以 能 安全 地 与 人 一 起 工 
作 “ 为 菜 ”， 工 业 智能 机 器 人 不 仅 在 机 械 结构 上 有 类 似 人 的 行走 、 转 腰 、 大 璧 、 小 辟 、 手 腕 、 手 爪 等 部 分 ， 还 有 许多 类 似 人 类 的 “生物 传感器 ”， 如 皮肤 型 接触 传感器 、 力 传感器 、 负 和 载 传感器 、 视 觉 与 声 
沉 传 感 器 等 ， 以 提高 自身 对 周围 环境 的 自 适应 能 力 。 例 如 ， 光 线 变 暗影 响 图 像 识 别 ， 传 送 带 上 物品 有 损坏 需要 特殊 处 理 的 自动 适应 能 力 ， 通 过 触觉 、 视 党、 听觉 等 感知 自主 判断 零 部 件 的 装配 质量 ， 等 等 ， 
如 图 1-8 所 示 。 
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图 1-8 ”工业 智能 机 器 人 的 发 展 


3) 注重 通用 和 易 用 性 : 除了 专门 设计 的 专用 工业 机 器 人 之 外 ， 一 般 工 业 智 能 机 器 人 在 执行 不 同 的 作业 任务 时 应 具有 较 好 的 通用 性 。 因 为 不 同 用 途 的 工业 智能 机 器 人 的 设计 要 求 各 不 相同 。 例 如 ， 有 的 需 
要 高 速 手臂 、 有 的 需要 慢 速 、 有 的 需要 扭力 大 、 有 的 需要 很 高 的 精密 度 等 ， 这 些 可 以 通过 更 换 未 端 操作 器 执行 不 同 的 作业 任务 ; 具有 方便 快捷 的 编程 能 力 ， 支 持 多 自由 度 运 动 ， 使 其 具有 更 强 的 应 用 场景 适 
应 性 ; 通用 性 还 体现 在 具有 统一 的 运行 时 平台 和 软件 开发 环境 ， 方 便 应 用 领域 开发 者 的 高 效 开 友 ; 工业 智能 机 器 人 的 易 用 性 也 开始 得 到 重视 ， 如 何 能 使 得 应 用 者 不 经 培训 或 少许 培训 ， 就 能 像 玩 iPhone 一 样 
很 快 掌握 应 用 工业 智能 机 器 人 的 技巧 ， 已 经 变 成 重要 的 发 展 方向 。 


3. 工 业 智 能 机 器 人 的 关键 技术 


随 着 智能 机 器 人 在 工业 生产 领域 应 用 的 不 断 扩 展 ， 为 适应 更 加 复杂 、 精 细 、 快 节拍 的 作业 ， 工 业 智 能 机 器 人 在 高 速度 、 高 精度 、 重 载荷 、 智 能 化 、 低 噪声 、 超 洁净 、 多 机 协调 等 方面 需要 进一步 提升 ， 
涉及 的 关键 技术 较 多 ， 主 要 体现 在 以 下 几 点 。 


(1) 三 维 环境 实时 感知 


通过 多 类 高 精度 的 传感器 及 其 多 传感器 信息 融合 ， 实 现 机 器 人 操作 对 象 和 运行 环境 的 在 线 实时 感知 能 力 ;不 仅 要 注重 对 已 有 传感器 的 高 效 合理 利用 ， 而 且 要 加 强 新 型 传感器 的 研制 和 开发 ， 新 型 传感器 
研发 的 着 重点 不 仅 是 开发 新 的 传感器 种 类 ， 更 是 努力 改善 已 有 传感器 的 精度 、 可 靠 度 和 成 本 ， 以 及 大 幅 提 高 传感器 的 性 能 价格 比 ; 多 传感器 信息 融合 是 指 综合 来 自 多 个 传感器 的 感知 数据 ， 以 产生 更 可 靠 、 
更 准确 或 更 全 面 的 信息 ， 经 过 融合 的 多 传感器 系统 能 够 更 加 完善 、 精 确 地 反映 检测 对 象 的 特性 ， 消 除 信息 的 不 确定 性 ， 提 高 信息 的 可 靠 性 ， 为 机 器 人 在 各 种 复杂 、 动 态 、 不 确定 和 未 知 的 环境 中 执行 任务 提 
供 一 种 有 效 的 技术 解决 途径 。 


(2) 智能 精准 控制 


工业 智能 机 器 人 是 一 个 非 线性 、 多 变量 的 控制 对 象 ， 结 合 了 位 置 、 力 和 矩 、 视 觉 等 信息 反馈， 柔顺 控制 、 力 位 混合 控制 、 视 觉 伺服 控制 等 ， 面 对 高 速度 、 高 精度 、 重 载荷 的 作业 需求 ， 机 器 人 的 智能 控制 
方法 成 为 研究 重点 ， 其 是 智能 机 器 人 能 够 自主 独立 完成 各 项 任务 的 基础 ， 可 使 机 器 人 的 行动 更 加 灵活 方便 、 复 杂 多 样 ， 并 能 够 有 效 克服 随机 扰动 ， 增 加 机 器 人 的 自由 独立 性 。 例 如 ， 模 糊 数学 、 计 算 机 科 
学 、 知 识 工程 等 多 学 科 相 互 渗透 形成 的 模糊 控制 在 机 器 人 的 建 模 、 控 制 、 柔 性 臂 的 控制 、 补 偿 控制 及 移动 机 器 人 路 径 规 划 等 方面 得 到 了 广泛 应 用 ; 神经 网 络 控制 技术 及 基于 遗传 算法 的 控制 技术 等 分 别 借 鉴 
了 生物 领域 相关 理论 ， 能 更 好 地 处 理 多 变量 、 非 线性 系统 的 精准 控制 问题 ， 也 成 为 工业 智能 机 器 人 智能 控制 的 研究 重点 ， 更 高 级 的 智能 控制 是 实现 工业 机 器 人 的 类 人 灵巧 操作 。 


(3) 智能 规划 与 精确 导航 


工业 智能 机 器 人 的 各 种 能 力 最 终 只 能 依靠 机 器 人 末端 严格 按照 预定 轨迹 运动 完成 作业 ， 因 此 轨迹 规划 的 结果 将 直接 影响 工业 智能 机 器 人 的 工作 效能 和 效率 ， 轨 迹 规 划 的 效率 和 自动 化 程度 则 将 直接 影响 
生产 准备 时 间 。 为 了 提高 工业 智能 机 器 人 的 智能 规划 与 精确 导航 ， 图 像 识 别 、 神 经 网 络 、 机 器 学 习 、 视 党 处 理 等 智能 方法 研究 成 为 重点 ， 随 着 大 数据 、 云 计算 技术 的 发 展 ， 也 将 会 为 工业 智能 机 器 人 的 智能 
规划 与 精确 导航 提供 更 多 新 的 思路 。 

(4) 行为 安全 保障 


随 着 工业 智能 机 器 人 越 来 越 多 地 应 用 于 柔性 生产 线 ， 而 生产 线 多 是 工业 智能 机 器 人 与 人 的 协同 操作 ， 高 度 安全 并 智能 规避 对 人 类 伤害 的 机 器 人 是 协作 顺利 展开 的 基本 前 提 。 因 此 ， 在 对 工业 智能 机 器 人 
的 设计 中 ， 必 须 在 具有 友好 “人 -机 ”交互 的 同时 ， 充 分 考虑 和 保护 操作 人 员 安 全 ， 这 样 才能 保障 实现 “人 -机 ”合作 、“ 人 -机 ”融合 的 目标 。 需 要 研究 自主 碰撞 检测 、 安 全 区 域 和 障碍 物 自动 规避 、 轻 量化 
人 机 协同 及 系统 安全 设计 等 。 


(5) 开放 式 可 定制 软件 平台 


工业 智能 机 器 人 不 仅 可 实现 搬运 、 焊 接 、 喷 漆 、 装 配 、 码 摊 等 自动 化 工业 生产 功能 ， 而 且 具 有 多 机 协同 、 人 机 融合 等 能 力 。 随 着 智能 化 程度 的 不 断 提升 ， 其 软件 规模 不 断 增长 ， 软 件 结构 日 趋 复杂 ， 软 
件 成 为 决定 机 器 人 运动 位 置 、 姿 态 轨 人 迹 、 操 作 顺 序 及 动作 时 间 等 功能 和 性 能 的 主要 因素 之 一 。 各 大 工业 机 器 人 厂商 均 开 发 了 各 自 的 机 器 人 软件 系统 ， 使 得 系统 集成 、 应 用 开发 、 用 户 培 训 、 领 域 推广 难度 增 
大 ， 而 采用 构件 化 、 平 台 化 的 软件 系统 不 仅 有 益 开发 和 推广 ， 也 有 利于 系统 成 本 降低 。 因 此 ， 工 业 智 能 机 器 人 的 开放 式 可 定制 的 软件 平台 成 为 发 展 方向 和 关键 技术 。 这 将 涉及 基于 Agent 的 机 器 人 软件 结 
构 、 可 复 用 和 共享 的 标准 化 软 构件 集 及 可 定制 的 机 器 人 智能 化 运行 支撑 平台 等 。 
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2.1 ”ROS 框架 


2.1.1 ”ROS 简介 


机 器 人 操作 系统 的 发 展 与 电子 计算 机 操作 系统 的 发 展 十 分 类 似 。 早 期 的 计算 机 没有 操作 系统 ， 开 发 人 员 需 要 直接 面 对 硬 件 ， 并 且 各 个 硬件 厂商 的 产品 可 能 提供 了 不 同 的 接口 和 使 用 方法 ， 给 开发 人 员 带 
来 了 很 多 麻烦 和 重复 性 工作 。 而 操作 系统 出 现 后 ， 每 一 种 操作 系统 都 有 自己 的 驱动 程序 模型 ， 硬 件 厂 商 只 需要 按照 驱动 模型 开发 驱动 程序 ， 用 户 使 用 计算 机 时 不 必 过 于 关心 底层 细节 。 


目前 智能 机 器 人 研发 领域 也 期 望 有 类 似 的 研发 过 程 : 如 果 每 个 设备 厂商 为 某 个 “通用 机 器 人 平台 ”开发 相应 的 支持 软件 ， 研 发 人 员 通 过 简单 的 配置 和 适 配 开 发 就 能 让 它们 协调 工作 ， 从 而 可 以 把 更 多 的 
时 间 投 入 到 应 用 设计 与 开发 之 中 。 同 时 ， 机 器 人 软件 研发 也 存在 很 多 共性 问题 ， 比 如 易 用 性 、 开 发 效率 、 跨 平台 、 多 编程 语言 、 分 布 式 部 署 、 代 码 可 重用 ， 等 等 。 随 着 机 器 人 领域 的 快速 发 展 和 复杂 化 ， 智 
能 机 器 人 的 各 项 能 力 正在 日 趋 完 善 ， 为 智能 机 器 人 编写 软件 也 变 得 越 来 越 复杂 繁重 。 


针对 这 些 问题 的 解决 需求 ， 机 器 人 操作 系统 应 运 而 生 。2010 年 Willow Garage 公 司 发 布 了 开源 机 器 人 操作 系统 ROS， 很 快 就 在 机 器 人 研究 领域 掀起 了 学 习 和 使 用 ROS 的 热潮 。 


与 Windows、Linux 等 传统 意义 上 的 计算 机 操作 系统 不 同 ， 机 器 人 操作 系统 (ROS) 并 不 具体 负责 计算 机 进程 管理 与 调度 。 实 际 上 它 是 一 系列 运行 在 上 述 计算 机 操作 系统 核心 上 ， 面 向 机 器 人 特点 的 运 
行 支撑 软件 和 软件 工具 集 。 例 如 ，ROS 通 过 提供 一 种 软件 点 对 点 通信 机 制 ， 使 得 开发 人 员 可 以 高 效 灵活 地 组 织 机 器 人 软件 模块 。 目 前 通过 开发 者 的 不 断 努 力 ，ROS 的 功能 包 集 合 已 经 非常 丰富 ， 所 支持 的 机 
器 人 种 类 也 越 来 越 多 。 


ROS 是 用 于 机 器 人 的 一 种 开源 的 “后 操作 系统 ”， 或 者 说 次 级 操作 系统 。 它 提供 了 类 似 于 操作 系统 所 提供 的 功能 ,包含 硬件 抽象 描述 、 设 备 管理 与 控制 、 通 用 功能 实现 、 程 序 间 的 消息 传递 、 程 序 发 行 
包 管 理 等 ， 它 也 提供 了 一 些 工具 程序 和 函数 库 用 于 获取 、 建 立 、 编 写 和 运行 机 器 人 程序 。 

ROS 的 主要 设计 目标 是 便于 机 器 人 研发 过 程 中 的 代码 复 用 。 因 此 ROS 采 用 了 一 种 分 布 式 架构 ， 各 个 软 构 件 可 以 各 自 独 立地 设计 ， 松 散 、 即 时 地 组 合 起 来 。 并 可 以 按照 功能 包 和 功能 包 集 的 方式 分 组 ， 因 
而 可 以 更 容易 地 分 享 和 发 布 。 


ROS 的 运行 架构 是 一 种 基于 socket 网 络 连接 的 松 耦合 架构 。 在 ROs 的 架构 中 ， 一 切 可 执行 的 程序 被 抽象 为 节点 (node) ， 它 可 以 是 传感器 数据 采集 程序 、 执 行 器 控制 程序 、 规 划算 法 程序 、 视 党 识别 程 
序 ， 等 等 。 各 个 节点 之 间 均 使 用 ROS 提 供 的 消息 传递 机 制 进行 通信 。ROS 支 持 多 种 类 型 的 通信 ， 包 括 基 于 服务 的 同步 RPC 通 信 、 基 于 Topic 的 异步 数据 流通 信 太 参数 服务 器 上 的 数据 存储 等 。 


作为 一 个 开放 源 代码 软件 系统 ，ROS 的 宗旨 是 构建 一 个 能 够 整合 不 同 研究 成 果 ， 实 现 算 法 发 布 、 代 码 重 用 的 通用 机 器 人 软件 平台 。 由 于 这 些 功 能 可 满足 广大 开发 者 的 需求 ， 因 此 ，ROs 得 到 了 广泛 应 
用 。 不 少 智能 机 器 人 研发 者 在 ROS 的 基础 上 开发 了 许多 如 运动 规划 、 定 位 导航 、 仿 真 、 感 知 等 高 层 功 能 软件 包 ， 使 得 这 一 软件 平台 的 功能 更 加 丰富 ， 发 展 更 加 迅速 。 目 前 ，ROS1.0 已 经 发 行 多 个 版 本 ， 同 时 
ROS2.0 也 已 经 发 行 了 。 如 今 使 用 ROS 进 行 软 件 开发 的 智能 机 器 人 已 多 达 上 百 种 ， 其 范围 涵盖 机 械 手 、 类 人 机 器 人 、 平 面 移动 机 器 人 、 无 人 驾驶 汽车 和 无 人 飞行 器 等 热点 领域 ， 而 且 大 部 分 传感器 在 ROS 上 都 
得 到 了 较 好 的 支持 。 基 于 ROS 开 发 的 应 用 程序 更 是 多 达 2000 余 种 ， 涵 盖 了 模拟 仿真 、 硬 件 驱动 、 环 境 感知 、 运 动 控制 等 各 个 方面 。ROS 已 经 逐步 成 为 智能 机 器 人 研发 领域 的 通用 性 软件 平台 。 


ROS 具 有 如 下 主要 特点 。 


1) 分 布 式 架 构 : ROSs 将 每 个 工作 进程 都 看 作 一 个 节点 ， 使 用 节点 管理 器 进行 统一 管理 ， 并 提供 了 一 套 消息 传递 机 制 。 这 种 架构 可 以 分 散 由 计算 机 视觉 和 语音 识别 等 CPU 密集 型 任务 带 来 的 实时 计算 压 
力 ， 也 能 够 适应 多 机 器 人 系统 遇 到 的 挑战 。 基 于 套 接 字 通 信 的 大 量 采 用 ，ROSs 具 备 分 布 式 计算 的 实现 能 力 。 在 任何 具备 网 络 连接 的 主机 上 都 可 运行 节点 程序 ， 这 些 主机 可 以 是 小 型 的 ARM 让 入 式 系统 ， 或 者 
是 具备 强大 运算 能 力 的 基站 服务 器 ， 甚 至 是 基于 Android 系 统 的 移动 手机 或 平板 电脑 。 这 些 异 构 计算 平台 均 可 成 为 智能 机 器 人 系统 的 一 部 分 。 


2) 多 语言 支持 : 由 于 所 有 节点 的 通信 都 是 通过 网 络 套 接 字 来 实现 的 ， 这 意味 着 只 要 能 够 提供 套 接 字 接 口 ， 节 点 程序 可 以 用 任何 编程 语言 来 实现 。ROS 不 依赖 特定 的 编程 语言 ， 它 目前 已 经 支持 多 种 现代 
编程 语言 ， 如 C++、Python 和 Lisp 已 经 在 ROS 中 得 到 广泛 应 用 ，Java 的 测试 性 支持 也 已 经 实现 。ROS 采 用 了 一 种 独立 于 编程 语言 的 接口 定义 语言 (IDL) ， 并 实现 了 多 种 编程 语言 对 IDL 的 封装 ， 使 得 各 个 不 
同 编程 语言 编写 的 “节点 ”之 间 也 能 透明 地 进行 消息 传递 。 


3) 良好 的 可 伸缩 性 : 使 用 ROS 进 行 机 器 人 研发 ， 既 可 以 简单 地 编写 一 两 个 节点 单独 运行 ， 又 可 以 通过 rospack、roslaunch 将 很 多 个 节点 组 织 成 一 个 更 大 的 工程 ， 指 定 它 们 之 间 的 依赖 关系 及 运行 时 的 
组 织 形式 。 


4) 源码 开放 : ROS 遵 循 BSD 协 议 ， 实 现 源码 开放 ， 对 个 人 、 商 业 应 用 及 修改 完全 免费 ， 这 也 是 使 其 具有 更 强生 命 力 的 主要 原因 之 一 。 
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与 Windows、Linux 等 传统 意义 上 的 计算 机 操作 系统 不 同 ， 机 器 人 操作 系统 (ROS) 并 不 具体 负责 计算 机 进程 管理 与 调度 。 实 际 上 它 是 一 系列 运行 在 上 述 计算 机 操作 系统 核心 上 ， 面 向 机 器 人 特点 的 运 
行 支撑 软件 和 软件 工具 集 。 例 如 ，ROS 通 过 提供 一 种 软件 点 对 点 通信 机 制 ， 使 得 开发 人 员 可 以 高 效 灵活 地 组 织 机 器 人 软件 模块 。 目 前 通过 开发 者 的 不 断 努 力 ，ROS 的 功能 包 集 合 已 经 非常 丰富 ， 所 支持 的 机 
器 人 种 类 也 越 来 越 多 。 


ROS 是 用 于 机 器 人 的 一 种 开源 的 “后 操作 系统 ”， 或 者 说 次 级 操作 系统 。 它 提供 了 类 似 于 操作 系统 所 提供 的 功能 ,包含 硬 件 抽象 描述 、 设 备 管理 与 控制 、 通 用 功能 实现 、 程 序 间 的 消息 传递 、 程 序 发 行 
包 管 理 等 ， 它 也 提供 了 一 些 工具 程序 和 函数 库 用 于 获 了 到、 建立、 编写 和 运行 机 器 人 程序 。 


ROS 的 主要 设计 目标 是 便于 机 器 人 研发 过 程 中 的 代码 复 用 。 因 此 ROS 采 用 了 一 种 分 布 式 架构 ， 各 个 软 构 件 可 以 各 自 独 立地 设计 ， 松 散 、 即 时 地 组 合 起 来 。 并 可 以 按照 功能 包 和 功能 包 集 的 方式 分 组 ， 
而 可 以 更 容易 地 分 享 和 发 布 。 


ROS 的 运行 架构 是 一 种 基于 Socket 网 络 连接 的 松 耦 合 架构 。 在 ROS 的 架构 中 ， 一 切 可 执行 的 程序 被 抽象 为 节点 (node) ， 它 可 以 是 传感器 数据 采集 程序 、 执 行 器 控制 程序 、 规 划算 法 程序 、 视 党 识别 程 
序 ， 等 等 。 各 个 节点 之 间 均 使 用 ROS 提 供 的 消息 传递 机 制 进行 通信 。ROS 支 持 多 种 类 型 的 通信 ， 包 括 基 于 服务 的 同步 RPC 通 信 、 基 于 Topic 的 异步 数据 流通 信 太 参数 服务 器 上 的 数据 存储 等 。 


作为 一 个 开放 源 代码 软件 系统 ，ROS 的 宗旨 是 构建 一 个 能 够 整合 不 同 研 究 成 果 ， 实 现 算 法 发 布 、 代 码 重用 的 通用 机 器 人 软件 平台 。 由 于 这 些 功 能 可 满足 广大 开发 者 的 需求 ， 因 此 ，ROS 得 到 了 广泛 应 
用 。 不 少 智能 机 器 人 研发 者 在 ROS 的 基础 上 开发 了 许多 如 运动 规划 、 定 位 导航 、 仿 真 、 感 知 等 高 层 功 能 软件 包 ， 使 得 这 一 软件 平台 的 功能 更 加 丰富 ， 发 展 更 加 迅速 。 目 前 ，ROS1.0 已 经 发 行 多 个 版 本 ， 同 时 
ROS2.0 也 已 经 发 行 了 。 如 今 使 用 ROS 进 行 软 件 开发 的 智能 机 器 人 已 多 达 上 百 种 ， 其 范围 涵盖 机 械 手 、 类 人 机 器 人 、 平 面 移 动机 器 人 、 无 人 驾驶 汽车 和 无 人 飞行 器 等 热点 领域 ， 而 且 大 部 分 传感器 在 ROS 上 都 
得 到 了 较 好 的 支持 。 基 于 ROS 开 发 的 应 用 程序 更 是 多 达 2000 余 种 ， 涵 盖 了 模拟 仿真 、 硬 件 驱动 、 环 境 感知 、 运 动 控制 等 各 个 方面 。ROS 已 经 逐步 成 为 智能 机 器 人 研发 领域 的 通用 性 软件 平台 。 


ROS 具 有 如 下 主要 特点 。 


1) 分 布 式 架 构 : ROS 将 每 个 工作 进程 都 看 作 一 个 节点 ， 使 用 节点 管理 器 进行 统一 管理 ， 并 提供 了 一 套 消 息 传递 机 制 。 这 种 架构 可 以 分 散 由 计算 机 视觉 和 语音 识别 等 CPU 密 集 型 任务 带 来 的 实时 计算 压 
力 ， 也 能 够 适应 多 机 器 人 系统 遇 到 的 挑战 。 基 于 套 接 字 通 信和 的 大 量 采用 ，ROS 具 备 分 布 式 计算 的 实现 能 力 。 在 任何 具备 网 络 连 接 的 主机 上 都 可 运行 节点 程序 ， 这 些 主机 可 以 是 小 型 的 ARM 嵌 入 式 系 统 ,或 者 
是 具备 强大 运算 能 力 的 基站 服务 器 ， 甚 至 是 基于 Android 系 统 的 移动 手机 或 平板 电脑 。 这 些 异 构 计算 平台 均 可 成 为 智能 机 器 人 系统 的 一 部 分 。 


2) 多 语言 支持 : 由 于 所 有 节点 的 通信 都 是 通过 网 络 套 接 字 来 实现 的 ， 这 意味 着 只 要 能 够 提供 套 接 字 接 口 ， 节 点 程序 可 以 用 任何 编程 语言 来 实现 。ROS 不 依赖 特定 的 编程 语言 ， 它 目前 已 经 支持 多 种 现代 
编程 语言 ， 如 C++、Python 和 Lisp 已 经 在 ROS 中 得 到 广泛 应 用 ，Java 的 测试 性 支持 也 已 经 实现 。ROS 采 用 了 一 种 独立 于 编程 语言 的 接口 定义 语言 (IDL) ， 并 实现 了 多 种 编程 语言 对 1DL 的 封装 ， 使 得 各 个 不 
同 编程 语言 编写 的 “节点 ”之 间 也 能 透明 地 进行 消息 传递 。 


3) 良好 的 可 伸缩 性 : 使 用 ROS 进 行 机 器 人 研发 ， 既 可 以 简单 地 编写 一 两 个 节点 单独 运行 ， 又 可 以 通过 rospack、roslaunch 将 很 多 个 节点 组 织 成 一 个 更 大 的 工程 ， 指 定 它 们 之 间 的 依赖 关系 及 运行 时 的 
组 织 形式 。 


4) 源码 开放 : ROSs 遵 循 BSD 协 议 ， 实 现 源码 开放 ， 对 个 人 、 商 业 应 用 及 修改 完全 免费 ， 这 也 是 使 其 具有 更 强生 命 力 的 主要 原因 之 一 。 


第 2 草 ROS 体系 架构 


2.1 ROS 框架 


2.1.1 ”ROS 简介 
机 器 人 操作 系统 的 发 展 与 电子 计算 机 操作 系统 的 发 展 十 分 类 似 。 早 期 的 计算 机 没有 操作 系统 ， 开 发 人 员 需 要 直接 面 对 硬 件 ， 并 且 各 个 硬件 厂商 的 产品 可 能 提供 了 不 同 的 接口 和 使 用 方法 ， 给 开发 人 员 带 
来 了 很 多 麻烦 和 重复 性 工作 。 而 操作 系统 出 现 后 ， 每 一 种 操作 系统 都 有 自己 的 驱动 程序 模型 ， 硬 件 厂 商 只 需要 按照 驱动 模型 开发 驱动 程序 ， 用 户 使 用 计算 机 时 不 必 过 于 关心 底层 细节 。 


目前 智能 机 器 人 研发 领域 也 期 望 有 类 似 的 研发 过 程 : 如 果 每 个 设备 厂商 为 某 个 “通用 机 器 人 平台 ”开发 相应 的 支持 软件 ， 研 发 人 员 通 过 简单 的 配置 和 适 配 开 发 就 能 让 它们 协调 工作 ， 从 而 可 以 把 更 多 的 
时 间 投 入 到 应 用 设计 与 开发 之 中 。 同 时 ， 机 器 人 软件 研发 也 存在 很 多 共性 问题 ， 比 如 易 用 性 、 开 发 效率 、 跨 平台 、 多 编程 语言 、 分 布 式 部 署 、 代 码 可 重用 ， 等 等 。 随 着 机 器 人 领域 的 快速 发 展 和 复杂 化 ， 智 
能 机 器 人 的 各 项 能 力 正在 日 趋 完 善 ， 为 智能 机 器 人 编写 软件 也 变 得 越 来 越 复杂 繁重 。 


针对 这 些 问题 的 解决 需求 ， 机 器 人 操作 系统 应 运 而 生 。2010 年 Willow Garage 公 司 发 布 了 开源 机 器 人 操作 系统 ROS， 很 快 就 在 机 器 人 研究 领域 掀起 了 学 习 和 使 用 ROS 的 热潮 。 


与 Windows、Linux 等 传统 意义 上 的 计算 机 操作 系统 不 同 ， 机 器 人 操作 系统 (ROS) 并 不 具体 负责 计算 机 进程 管理 与 调度 。 实 际 上 它 是 一 系列 运行 在 上 述 计算 机 操作 系统 核心 上 ， 面 向 机 器 人 特点 的 运 
行 支撑 软件 和 软件 工具 集 。 例 如 ，ROS 通 过 提供 一 种 软件 点 对 点 通信 机 制 ， 使 得 开发 人 员 可 以 高 效 灵活 地 组 织 机 器 人 软件 模块 。 目 前 通过 开发 者 的 不 断 努 力 ，ROS 的 功能 包 集 合 已 经 非常 丰富 ， 所 支持 的 机 
器 人 种 类 也 越 来 越 多 。 


ROS 是 用 于 机 器 人 的 一 种 开源 的 “后 操作 系统 ”， 或 者 说 次 级 操作 系统 。 它 提供 了 类 似 于 操作 系统 所 提供 的 功能 ,包含 硬件 抽象 描述 、 设 备 管理 与 控制 、 通 用 功能 实现 、 程 序 间 的 消息 传递 、 程 序 发 行 
包 管 理 等 ， 它 也 提供 了 一 些 工具 程序 和 函数 库 用 于 获取 、 建 立 、 编 写 和 运行 机 器 人 程序 。 


ROS 的 主要 设计 目标 是 便于 机 器 人 研发 过 程 中 的 代码 复 用 。 因 此 ROS 采 用 了 一 种 分 布 式 架构 ， 各 个 软 构 件 可 以 各 自 独 立地 设计 ， 松 散 、 即 时 地 组 合 起 来 。 并 可 以 按照 功能 包 和 功能 包 集 的 方式 分 组 ， 
而 可 以 更 容易 地 分 享 和 发 布 。 


ROS 的 运行 架构 是 一 种 基于 Socket 网 络 连接 的 松 耦 合 架构 。 在 ROS 的 架构 中 ， 一 切 可 执行 的 程序 被 抽象 为 节点 (node) ， 它 可 以 是 传感器 数据 采集 程序 、 执 行 器 控制 程序 、 规 划算 法 程序 、 视 党 识别 程 
序 ， 等 等 。 各 个 节点 之 间 均 使 用 ROS 提 供 的 消息 传递 机 制 进行 通信 。ROS 支 持 多 种 类 型 的 通信 ， 包 括 基 于 服务 的 同步 RPC 通 信 、 基 于 Topic 的 异步 数据 流通 信 太 参数 服务 器 上 的 数据 存储 等 。 


作为 一 个 开放 源 代码 软件 系统 ，ROS 的 宗旨 是 构建 一 个 能 够 整合 不 同 研究 成 果 ， 实 现 算 法 发 布 、 代 码 重用 的 通用 机 器 人 软件 平台 。 由 于 这 些 功 能 可 满足 广大 开发 者 的 需求 ， 因 此 ，ROS 得 到 了 广泛 应 
用 。 不 少 智能 机 器 人 研发 者 在 ROS 的 基础 上 开发 了 许多 如 运动 规划 、 定 位 导航 、 仿 真 、 感 知 等 高 层 功 能 软件 包 ， 使 得 这 一 软件 平台 的 功能 更 加 丰富 ， 发 展 更 加 迅速 。 目 前 ，ROS1.0 已 经 发 行 多 个 版 本 ， 同 时 
ROS2.0 也 已 经 发 行 了 。 如 今 使 用 ROS 进 行 软 件 开发 的 智能 机 器 人 已 多 达 上 百 种 ， 其 范围 涵盖 机 械 手 、 类 人 机 器 人 、 平 面 移 动机 器 人 、 无 人 驾驶 汽车 和 无 人 飞行 器 等 热点 领域 ， 而 且 大 部 分 传感器 在 ROS 上 都 
得 到 了 较 好 的 支持 。 基 于 ROS 开 发 的 应 用 程序 更 是 多 达 2000 余 种 ， 涵 盖 了 模拟 仿真 、 硬 件 驱动 、 环 境 感知 、 运 动 控制 等 各 个 方面 。ROS 已 经 逐步 成 为 智能 机 器 人 研发 领域 的 通用 性 软件 平台 。 


ROS 具 有 如 下 主要 特点 。 


1) 分 布 式 架 构 : ROSs 将 每 个 工作 进程 都 看 作 一 个 节点 ， 使 用 节点 管理 器 进行 统一 管理 ， 并 提供 了 一 套 消 息 传递 机 制 。 这 种 架构 可 以 分 散 由 计算 机 视觉 和 语音 识别 等 CPU 密集 型 任务 带 来 的 实时 计算 压 
力 ， 也 能 够 适应 多 机 器 人 系统 遇 到 的 挑战 。 基 于 套 接 字 通 信 的 大 量 采 用 ，ROS3s 具 备 分 布 式 计算 的 实现 能 力 。 在 任何 具备 网 络 连接 的 主机 上 都 可 运行 节点 程序 ， 这 些 主机 可 以 是 小 型 的 ARM 谈 入 式 系统 ， 或 者 
是 具备 强大 运算 能 力 的 基站 服务 器 ， 甚 至 是 基于 Android 系 统 的 移动 手机 或 平板 电脑 。 这 些 异 构 计算 平台 均 可 成 为 智能 机 器 人 系统 的 一 部 分 。 


2) 多 语言 支持 : 由 于 所 有 节点 的 通信 都 是 通过 网 络 套 接 字 来 实现 的 ， 这 意味 着 只 要 能 够 提供 套 接 字 接口 ， 节 点 程序 可 以 用 任何 编程 语言 来 实现 。ROS 不 依赖 特定 的 编程 语言 ， 它 目前 已 经 支持 多 种 现代 
编程 语言 ， 如 C++、Python 和 Lisp 已 经 在 ROS 中 得 到 广泛 应 用 ，Java 的 测试 性 支持 也 已 经 实现 。ROS 采 用 了 一 种 独立 于 编程 语言 的 接口 定义 语言 (IDL) ， 并 实现 了 多 种 编程 语言 对 IDL 的 封装 ， 使 得 各 个 不 
同 编程 语言 编写 的 “节点 ”之 间 也 能 透明 地 进行 消息 传递 。 


3) 良好 的 可 伸缩 性 : 使 用 ROS 进 行 机 器 人 研发 ， 既 可 以 简单 地 编写 一 两 个 节点 单独 运行 ， 又 可 以 通过 rospack、roslaunch 将 很 多 个 节点 组 织 成 一 个 更 大 的 工程 ， 指 定 它 们 之 间 的 依赖 关系 及 运行 时 的 
组 织 形式 。 


4) 源码 开放 : ROS 遵 循 BSD 协 议 ， 实 现 源码 开放 ， 对 个 人 、 商 业 应 用 及 修改 完全 免费 ， 这 也 是 使 其 具有 更 强生 命 力 的 主要 原因 之 一 。 


2.1.2 ROS 整体 架构 分 析 


ROS 系 统 架构 主要 被 设计 划分 为 三 个 级 别 ， 如 图 2-1 所 示 。 
- 文件 系统 级 (Filesystem level) ， 用 于 描述 可 以 在 硬盘 上 查 到 的 代码 及 可 执行 程序 。 
- 计算 图 级 (Computation Graph level) ， 体 现 的 是 进程 与 进程 、 进 程 与 系统 之 间 的 通信 。 


- 开源 社区 级 (Community level) ， 主 要 包括 在 开发 人 员 之 间 如 何 共 享 知 识 、 算 法 和 代码 。 
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图 2-1 ROS 系 统 架 构 
1. 文 件 系 统 级 
ROSs 中 有 众多 的 抽象 节点 及 消息 、 服 务 、 工 具 和 库 文 件 ， 需 要 采用 有 效 的 结构 管理 这 些 代 码 。 一 个 ROS 程 序 的 不 同 构件 被 放 在 不 同 的 文件 夹 下 ， 这 些 文 件 夹 是 根据 功能 的 不 同 来 对 文件 进行 组 织 能 


ROS 的 软件 是 以 Package (功能 包 ) 的 方式 进行 组 织 的 。ROS 系 统 由 众多 的 功能 包 组 成 ， 每 个 功能 包 中 可 能 包含 多 个 节点 的 可 执行 文件 、 消 息 接口 定义 文件 、RPC 服 务 接口 定义 文件 及 库 文件 等 。 此 外 
还 包含 一 个 功能 包 清 单 ， 如 图 2-2 所 示 。 


功能 包 之 间 可 以 配置 依赖 关系 。 如 果 设 定 了 功能 包 A 依 赖 功能 包 B， 那 么 ROS 在 构建 系统 时 ，B 一 定 要 早 于 A 构建 ， 并 且 在 编译 A 时 ， 自 动 配置 环境 ， 使 得 A 可 以 使 用 B 中 的 头 文件 和 库 文件 。 
(1) 功能 包 集 (Stack) 


将 一 些 具 有 某 些 相 关 功 能 的 功能 包 组 织 在 一 起 ， 就 是 一 个 功能 包 集 。 在 ROS 中 存在 大 量 的 不 同 用 途 的 功能 包 集 ， 如 导航 功能 包 集 navigation、 机 械 辟 运动 控制 功能 包 集 Movelt。 








DREE 








图 2-2 ”ROS 文件 系统 级 
功能 包 集 必须 包含 三 个 文件 : CMakeList.txt、Makefile 和 stack.xml。 如 果 在 文件 夹 下 见 到 stack.xml， 那 么 可 以 确定 这 是 一 个 功能 包 集 
(2) 功能 包 (Package) 
功能 包 是 ROS 中 软件 组 织 的 基本 形式 。 创 建 ROS 程 序 时 ， 功 能 包 具有 最 小 的 结构 和 最 少 的 内 容 ， 它 可 以 包含 ROS 运 行 的 进程 (节点) 、 配 置 文件 等 。 
当 提 到 功能 包 时 ， 一 般 指 的 是 一 种 特定 的 文件 结构 和 文件 夹 组 合 。 这 种 结构 具体 如 下 所 示 。 


bin/: 编译 和 链接 程序 后 ， 用 于 存储 可 执行 文件 的 文件 夹 。 


- include/package_name/: 此 目录 包含 了 所 需 库 的 头 文件 。 请 记 住 导 出 功能 包 清 单 ， 因 为 它们 还 会 被 其 他 功能 包 所 使 用 。 

. msg/: 程序 开发 者 开发 的 非 标准 消息 类 型 。 

“ sctipts/: 包括 bash、Python 或 任何 其 他 脚本 的 可 执行 脚本 文件 。 

“stc/: 程序 源 文件 ， 可 以 为 节点 创建 一 个 文件 来 ， 或 者 按照 希望 的 方式 组 织 它 。 

' stV/: 程序 开发 者 开发 的 非 标准 服务 。 

- CMakeLists.txt: CMake 的 生成 文件 。 

manifest.xml/package.xml: 功能 包 清 单 文件 ， 其 必须 包含 在 功能 包 中 ， 用 来 说 明 此 功能 包 相 关 的 各 类 信息 。 如 果 你 发 现 某 个 文件 夹 内 包含 了 此 文件 ， 那 么 这 个 文件 夹 很 可 能 是 一 个 功能 包 。 
(3) 功能 包 清单 (Manifest) 


功能 包 清单 提供 关于 功能 包 、 许 可 信息 、 依 赖 关 系 、 编 译 标 志 等 的 信息 。 功 能 包 清 单 是 一 个 manifests.xml/package.xm| 文 件 ， 通 过 这 个 文件 能 够 实现 对 功能 包 的 管理 。 打 开 一 个 功能 包 清 单 文 件 ， 可 
以 看 到 包 的 名 称 、 依 赖 关系 等 信息 。 功 能 包 清 单 的 作用 就 是 为 了 更 容易 地 安装 和 分 发 这 些 功 能 包 。 


(4) 消息 类 型 (Message/msg type) 


消息 是 ROS 数 据 传递 的 基本 元 素 ， 消 息 类 型 需要 用 ROS 的 消息 定义 语言 进行 描述 。 每 个 功能 包 的 msg 文 件 夹 中 ， 都 定义 了 这 个 功能 包 需 要 的 消息 类 型 。 此 外 ， 如 果 该 功能 包 依 赖 男 外 一 个 功能 包 ， 则 该 
功能 包 可 以 使 用 另 一 功能 包 的 所 有 消息 类 型 。 


ROS 使 用 了 一 种 简化 的 消息 类 型 描述 语言 来 描述 ROS 节 点 发 布 的 数据 值 。 通 过 这 样 的 描述 语言 ，ROS 能 够 使 用 多 种 编程 语言 生成 不 同 消息 类 型 的 代码 。 


ROS 提 供 了 很 多 预定 义 的 消息 类 型 。 如 果 你 创建 了 一 种 新 型 消息 类 型 ， 那 么 就 要 把 消息 类 型 的 定义 放 到 功能 包 的 msg 文 件 夹 下 。 该 文件 夹 中 包含 了 用 于 定义 各 种 消息 的 文件 ， 这 些 文件 都 以 .msg 为 扩展 


(5) 服务 类 型 (Service/srv type) 
ROS 使 用 了 一 种 简化 的 服务 描述 语言 来 描述 ROS 的 服务 类 型 。 这 直接 借鉴 了 ROS 消 息 的 数据 格式 ， 以 实现 节点 之 间 的 请 求 /响应 通信 。 服 务 的 描述 存储 在 功能 包 的 srv 子 目录 下 扩展 名 为 .srv 的 文件 中 。 
若 要 调用 服务 ， 调 用 者 需要 使 用 该 服务 功能 包 的 名 称 及 服务 名 称 。 例 如 ， 对 于 sample_package1/srv/sample1.srv 文 件 ， 可 以 将 它 称 为 saample_package1/sample1 服 务 。 


ROS 中 有 查看 某 些 功能 包 名 称 与 服务 的 相关 工具 。 例 如 ，rossrv 工 具 能 输出 服务 说 明 .srv 文 件 所 在 的 功能 包 名 称 ， 并 可 以 找到 使 用 某 一 服务 类 型 的 源 代 码 文件 。 如 果 要 在 ROS 中 创建 一 个 服务 ， 可 以 使 用 
服务 生成 器 。 这 些 工具 能 够 从 基本 的 服务 说 明 中 生成 代码 ， 只 需要 在 CMakeLists.txt 文 件 中 加 一 行 gensrv () 命令 即 可 。 


2. 计 算 图 级 


ROS 能 够 创建 一 个 连接 所 有 进程 的 抽象 “网 络 ”， 该 网 络 是 进程 (节点 ) 之 间 通 过 主题 或 服务 的 连接 所 形成 的 。 系 统 中 的 任何 节点 都 可 以 访问 此 网 络 ， 并 通过 该 网 络 与 其 他 节点 进行 交互 ， 获 取 其 他 节 
点 发 布 的 消息 ， 并 将 自身 数据 发 布 到 网 络 上 。 


在 这 一 层级 中 最 基本 的 概念 实体 包括 节点 、 节 点 管理 器 、 参 数 服务 器 、 消 息 、 服 务 、 主 题 和 消息 记录 包 ， 如 图 2-3 所 示 ， 它 们 均 以 不 同 的 方式 向 计算 图 级 提供 数据 。 
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图 2-3 ”ROS 计算 图 级 


(1) 节点 (Node) 


节点 是 主要 的 计算 执行 进程 。 如 果 想 要 有 一 个 可 以 与 其 他 节点 进行 交互 的 进程 ， 那 么 需要 创建 一 个 节点 ， 并 将 此 节点 连接 到 ROS 网 络 中 。 通 常情 况 下 ， 系 统 包含 能 够 实现 不 同 功能 的 多 个 节点 。 最 好 让 
每 一 个 节点 都 具有 特定 的 单一 功能 ， 而 不 是 在 系统 中 创建 一 个 包罗 万 象 的 “大 节点 ”。 


节点 都 是 各 自 独 立 的 可 执行 文件 ， 并 能 够 通过 主题 、 服 务 或 参数 服务 器 与 其 他 节点 进行 通信 。ROS 通 过 使 用 节点 概念 将 代码 和 功能 解 耦 ， 提 高 了 系统 的 容错 能 力 和 可 维护 性 ， 使 系统 设计 得 以 简化 。 同 
时 ， 节 点 允许 ROS 能 够 布置 在 任意 机 器 上 并 同时 运行 。 


节点 在 系统 中 必须 有 唯一 的 名 称 ， 以 便 节 点 使 用 特定 名 称 与 其 他 节点 进行 通信 而 不 产生 歧义 。 节 点 可 以 使 用 不 同 的 库 进 行 编写 ， 如 roscpp 和 rospy，roscpp 基 于 C++，rospy 基 于 Python。 


ROS 是 由 很 多 节点 组 成 的 。 在 这 里 节点 实际 上 是 “ 软 构件 ”。ROS 使 用 节点 使 得 基于 ROS 的 系统 在 运行 时 更 加 形象 化 : 当 许 多 节点 同时 运行 时 ， 可 以 方便 地 使 用 rqt_graph 工 具 将 节点 与 节点 之 间 的 通信 


绘制 成 一 个 图 表 ， 其 中 进程 就 是 图 中 的 节点 ， 节 点 间 的 消息 连接 关系 就 是 其 中 的 弧 线 连接 ， 如 图 2-4 所 示 。 
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图 2-4 ROS 节 点 示意 


ROS 提 供 了 处 理 节 点 的 工具 ， 如 rosnode 是 一 个 用 于 显示 节点 信息 的 命令 行 工 具 ， 支 持 的 主要 命令 具体 如 下 。 


rosnode info node: 输出 当前 节点 的 信息 。 


- rosnode kill node: 结束 当前 运行 的 节点 进程 。 


- rosnode list: 列 出 当前 活动 的 节点 。 


“ rosnode machine hostname: 列 出 某 一 特定 计算 机 上 运行 的 节点 或 列 出 主机 名 称 。 


- rosnode ping node: 测试 节点 间 的 连通 性 。 


- rosnode cleanup: 将 无 法 访问 节点 的 注册 信息 清除 。 


ROS 节 点 的 一 个 强大 功能 是 在 启动 该 节点 时 更 改 参数 ， 此 功能 使 我 们 能 够 改变 节点 、 主题 和 参数 的 名 称 。 我 们 无 需 重新 编译 代码 就 能 重新 配置 节点 ， 这 样 就 可 以 在 不 同 的 场景 中 使 用 该 节 


以 下 为 一 个 改变 主题 名 称 的 示例 : 


$ rosrun book tutorials tutorialX topicl:=/levell/topicl 





此 命令 将 主题 名 称 从 topic1 改 为 /level1/topic1。 也 许 你 现在 还 不 明白 其 作用 ， 但 在 以 后 的 应 用 中 你 会 发 现 它 的 实用 性 。 


节点 中 的 参数 与 更 改 主题 名 称 很 类 似 。 只 需要 在 参数 名 称 前 添加 一 个 下 划 线 ， 例 如 : $rosrun book tutorials tutorialX_param: =9.0。 但 需要 注意 的 是 ， 不 能 使 用 系统 保留 关键 字 的 名 称 ， 具 体 
如 下 。 


_ name: 为 节点 名 称 保留 的 一 个 特殊 关键 字 。 
_ log: 为 记录 节点 日 志 存 储 地 址 保留 的 一 个 关键 字 。 
_ ip hostname: 表示 ROS_IP 和 ROS_HOSTNAME 的 关键 字 。 
-_master: 表示 ROS_MASTER_URI 的 关键 字 。 
ns: 表示 ROS_NAMESPACE 的 关键 字 。 
(2) 节点 管理 器 (Master) 


节点 管理 器 用 于 主题 、 服 务 名 称 的 注册 和 查找 等 。 如 果 在 整个 ROS 中 没有 节点 管理 器 ， 就 不 会 有 节点 间 的 通信 。 需 要 注意 的 是 ， 由 于 ROS 本 身 就 是 一 个 分 布 式 网 络 系统 ， 可 以 在 某 一 台 计 算 机 上 运行 节 
点 管理 器 ， 在 其 他 计算 机 上 运行 由 该 管理 器 管理 的 节点 。 节 点 管理 器 通常 使 用 roscore 命 令 运行 ， 它 会 加 载 KOS 节 点 管理 器 及 其 他 ROS 核 心 构件 。 同 时 ， 节 点 管理 器 还 提供 了 参数 服务 器 。 


(3) 参数 服务 器 (Parameter Server) 
参数 服务 器 是 可 通过 网 络 访问 的 共享 的 多 变量 字典 ， 通 过 关键 字 存 储 在 节点 管理 器 上 。 节 点 使 用 此 服务 器 存储 和 检索 运行 时 的 参数 ， 还 可 以 改变 节点 的 工作 任务 。 
参数 服务 器 使 用 XMLRPC 实 现 ， 并 运行 在 ROS 节 点 管理 器 上 。 这 意味 着 其 API 可 通过 通用 的 XMLRPC 库 进行 访问 。 
ROS 中 关于 参数 服务 器 的 工具 是 rosparam， 其 支持 的 参数 具体 如 下 。 

rosparam list: 列 出 参数 服务 器 中 的 所 有 参数 。 

- rosparam get parameter: 获取 参数 值 。 

- rosparam set parameter value: 设置 参数 值 。 

- rosparam delete parameter: 删除 参数 。 

` tospatam dump file: 将 参数 服务 器 保存 到 一 个 文件 中 。 

. rosparam load file: 加 载 参 数 文件 到 参数 服务 器 。 


(4) 消息 (Message) 


节点 可 通过 消息 实现 彼此 的 逻辑 联系 与 数据 交换 。 消 息 包 含 一 个 节点 发 送 到 其 他 节点 的 数据 信息 。 消 息 具有 多 种 标准 类 型 ， 同 时 用 户 也 可 以 基于 标准 消息 开发 自 定义 类 型 的 消息 。 
ROS 使 用 命令 行 工具 rosmsg 来 获取 有 关 消 息 的 信息 ， 主 要 参数 具体 如 下 。 

rosmsgshow: Z&—-FW MHP. 

- rosmsglist: AIRAA NA 

- rosmsg package: 列 出 功能 包 中 的 所 有 消息 。 

- rosmsg packages: ARAARA ETEVA Le 

- rosmsg users: 搜索 使 用 该 消息 类 型 的 代码 文件 。 

-rosmsgmd5: 显示 一 条 消息 的 MD5 检 验 值 。 

(5) 主题 (Topic) 


主题 是 由 ROS 网 络 对 消息 进行 路 由 和 消息 管理 的 “数据 总 线 ”。 每 一 条 消息 都 要 发 布 到 相应 的 主题 上 。 当 一 个 节点 发 送 数据 时 ， 就 说 该 节点 正在 向 主题 发 布 消息 。 节 点 可 以 通过 订阅 某 个 主题 ， 接 收 来 
自 其 他 节点 的 消息 。 节 点 可 以 订阅 任何 节点 发 布 的 主题 ， 而 不 需要 了 解 向 该 主题 发 送 消息 的 节点 。 这 就 保证 了 消息 的 发 布 者 和 订阅 者 之 间 相 互 解 厢 ， 完 全 无 需 知 晓 对 方 的 存在 。 主 题 的 名 称 必须 具有 了 唯一 
性 ， 否 则 在 同名 主题 之 间 的 消息 路 由 就 会 发 生 错误 。 


每 一 个 主题 都 是 强 类 型 的 ， 发 布 到 主题 上 的 消息 必须 与 主题 的 ROS 消 息 类 型 相 匹配 ， 并 且 节 点 只 能 接收 类 型 匹配 的 消息 。 一 个 节点 要 订阅 一 个 主题 ， 相 关 者 必须 具有 相同 的 消息 类 型 。 


ROS 的 主题 消息 可 以 使 用 TCP/IP 或 UDP 传 输 。 基 于 TCP 传 输 称 为 TCPROS， 它 使 用 TCP/IP 长 连接 ， 这 是 ROS 默 认 的 传输 方式 。 基 于 UDP 传 输 称 为 UDPROS， 它 是 一 种 低 延 迟 、 高 效率 的 传输 方式 ,但 可 
能 产生 数据 丢失 ， 所 以 它 适合 于 远程 操控 任务 。 


ROS 提 供 了 rostopic 命 令 行 工具 用 于 主题 操作 ， 人 允许 获取 主题 的 相关 信息 或 直接 在 网 络 上 发 布 数据 。 此 工具 的 参数 具体 如 下 。 
- rostopic bw/topic: 显示 主题 所 使 用 的 带宽 。 
- tostopic echo/topic: 将 主题 对 应 的 消息 输出 到 屏幕 。 
- tostopic find message_type: 按照 类 型 查找 主题 。 
- rostopic hz/topic: 显示 主题 的 发 布 频率 。 
- rostopic info/topic: 输出 关于 该 主题 的 信息 。 
.tostopic list: 输出 活动 主题 列表 。 
- rostopic pub/topic type args: 将 数据 发 布 到 主题 。 它 允许 我 们 直接 从 命令 行 中 对 任意 主题 创建 和 发 布 数据 。 
. rostopic type/topic: 输出 主题 的 类 型 ， 或 者 说 主题 中 发 布 的 消息 类 型 。 
(6) 服务 (Service) 
服务 用 于 请 求 应 答 模型 ， 也 必须 有 一 个 唯一 的 名 称 。 当 一 个 节点 提供 某 个 服务 时 ， 所 有 的 节点 都 可 以 通过 使 用 ROS 客 户 端 库 所 编写 的 代码 与 之 通信 。 


ROS 关 于 服务 的 两 个 命令 行 工具 是 rossrv 和 rosservice。 可 以 通过 rossrv 看 到 有 关 服 务 数 据 结 构 的 信息 ， 并 且 与 rosmsg 具 有 完全 一 致 的 用 法 ; 通过 rosservice 可 以 列 出 服务 列表 和 查询 某 个 服务 。 其 所 支 
持 的 命令 具体 如 下 。 


- rossetvice call/service args: 根据 命令 行 参数 调用 服务 。 
“rosservice find msg-type: 根据 服务 类 型 查询 服务 。 

- fossetvice info/service: 输出 服务 信息 。 

- rosservice list: 输出 活动 服务 列表 。 

“rosservice type/setvice: 输出 服务 类 型 。 

- tossetvice uti/setvice: 输出 服务 的 ROSRPCURI。 


(7) 消息 记录 包 (Bag) 


消息 记录 包 是 一 种 用 于 保存 和 回放 ROS 消 息 数据 的 文件 格式 ， 保 存在 .bag 文 件 中 。 消 息 记 录 包 是 一 种 用 于 存储 数据 的 重要 机 制 ， 它 能 够 获取 并 记录 各 种 难以 收集 的 传感器 数据 。 程 序 可 以 通过 消息 记录 
包 反 复 获取 实验 数据 ， 以 进行 必要 的 开发 和 算法 测试 。 在 使 用 复杂 机 器 人 进行 实验 工作 时 ， 需 要 经 常 使 用 消息 记录 包 。 


消息 记录 包 文 件 可 以 像 实时 会 话 一 样 在 ROS 中 再 现 情景 ， 在 相同 时 间 内 向 主题 发 送 相同 的 数据 。 通 常情 况 下 ， 开 发 者 可 以 使 用 此 功能 来 调试 算法 。 若 要 使 用 记录 包 文 件 ， 则 可 以 使 用 以 下 ROS 工 具 。 
. rosbag: 用 来 录制 、 播 放 和 执行 其 他 操作 。 

. rxbag: 用 于 可 视 化 图 形 环境 中 的 数据 。 

` rostopic: 用 于 查看 节点 发 送 的 主题 。 

3. 开 源 社 区 级 

ROS 开 源 社区 级 的 概念 主要 用 于 ROS 资 源 管 理 ， 其 能 够 通过 独立 的 网 络 社区 分 享 软件 和 知识 。 这 些 资源 具体 如 下 。 


- 发 行 版 (Distribution) : ROS 发 行 版 是 可 以 独立 安装 、 带 有 版 本 号 的 一 系列 功能 包 集 。ROS 发 行 版 像 Linux 发 行 版 一 样 可 发 挥 类 似 的 作用 。 这 使 得 ROS 软 件 安装 更 加 容易 ， 而 且 能 够 通过 一 个 软件 集合 
来 维持 一 致 的 版 本 。 


“ 软件 源 (Repositorie) : ROS 依 赖 于 共享 代码 与 软件 源 的 网 站 或 主机 服务 ， 在 这 里 不 同 的 机 构 都 能 够 发 布 和 分 享 各 自 的 机 器 人 软件 和 程序 。 


- ROS Wiki: ROS Wiki 是 用 于 记录 有 关 ROS 系 统 信息 的 主要 论坛 。 任 何人 都 可 以 注册 账户 和 贡献 自己 的 文件 、 提 供 更 正 或 更 新 、 编 写 教程 及 其 他 信息 。 


- 邮件 列表 (Mailinglist) : ROS 用 户 邮 件 列表 是 关于 ROS 的 主要 交流 渠道 ， 能 够 交流 从 ROS 软 件 更 新 到 ROS 软 件 使 用 中 的 各 种 疑惑 或 信息 。 


2.1.3 名称 系统 

当 构 建 一 个 规模 较 大 且 结 构 复杂 的 智能 机 器 人 系统 时 ，ROS 框 架 通 过 名 称 来 处 理 和 提取 复杂 的 信息 。 因 此 名 称 在 ROS 中 具有 非常 重要 的 作用 : 节点 、 主 题 、 服 务 和 参数 均 有 各 自 的 名 称 。 每 一 个 客户 端 
库 都 支持 名 称 的 命令 行 再 映射 ， 这 意味 着 一 个 编译 过 的 程序 可 以 在 运行 时 被 重新 构建 ， 以 便于 其 能 够 在 不 同 的 图 级 拓扑 结构 中 操作 。 

1. 计 算 图 资源 名 称 

计算 图 资源 名 称 提供 了 分 层 命名 结构 ， 用 于 在 ROS 中 计算 图 中 的 所 有 “资源 ”， 如 节点 、 参 数 、 主 题 和 服务 。 计 算 图 资源 名 称 的 机 制 在 ROS 中 非常 有 用 ， 对 于 构建 复杂 的 机 器 人 系统 有 很 大 的 帮助 。 

以 下 是 一 些 计算 图 资源 名 称 的 具体 实例 。 

- /foo 

- /nwpu/robot/name 

- /ur5/nodel 


计算 图 资源 名 称 提供 了 封装 作用 。 每 一 个 资源 被 定义 在 一 个 命名 空间 内 ， 该 资源 可 以 与 其 他 资源 共享 。 一 般 来 说 ， 资 源 可 以 在 它们 的 命名 空间 内 被 创建 ， 它 们 在 自己 的 命名 空间 内 或 上 一 级 命名 空间 内 
连接 资源 。 连 接 可 以 在 不 同 的 命名 空间 的 资源 之 间 进 行 ， 不 过 这 通常 需要 编写 一 段 代码 来 实现 。 这 种 封装 分 离 了 系统 的 不 同 部 分 ， 从 而 避免 了 因为 偶然 的 名 称 错误 或 全 局 的 “名 称 动 持 ”而 导致 出 现 不 期 望 
的 结果 。 


资源 名 称 的 解析 可 以 是 相对 的 ， 所 以 资源 并 不 一 定 需要 知道 它们 在 哪个 命名 空间 内 。 这 样 可 以 简化 节点 的 编写 ， 在 编写 每 个 节点 时 可 以 假设 它们 就 在 根 命名 空间 内 。 当 将 这 样 编写 的 节点 集成 到 更 大 的 
系统 中 时 ， 它 们 可 以 被 放 到 另 一 个 指定 的 命名 空间 内 。 


例如 ， 有 两 个 开发 者 A 和 B， 合 作 开 发 一 个 智能 机 器 人 的 软件 系统 。A 开 发 的 数 个 节点 与 B 开 发 的 数 个 节点 中 有 一 个 节点 名 字 重 复 (如 /Camera_node) ， 如 果 要 将 A 与 B 的 成 果 部 署 到 同一 台 智 能 机 器 人 
上 ， 那 么 只 需 将 A、B 的 节点 放 入 不 同 的 命名 空间 内 ， 即 可 区 分 同名 的 节点 。 如 : 


/DeveloperA/Camera node 
/DeveloperB/Camera node 











此 外 ， 这 种 机 制 也 在 很 大 程度 上 增强 了 ROS 的 可 复 用 性 。 例 如 ， 我 们 为 一 个 双 目 摄像 头 开 发 了 一 个 图 像 采集 节点 ,该 节点 发 布 了 两 个 主题 : /Camera_Left 与 /Camera_Right。 若 智能 机 器 人 需要 安装 两 
台 这 种 摄像 头 同时 工作 ， 这 时 可 以 在 两 个 不 同 的 命名 空间 (例如 CamA、CamB) 内 分 别 运行 相同 的 节点 ， 这 样 该 节点 发 布 的 主题 也 在 不 同 的 命名 空间 内 ， 如 : 


摄像 头 A: /CamA/Camera Left 与 /CamA/Camera Right 

摄像 头 B: /CamB/Camera Left 与 /CamB/Camera Right 

2. 名 称 的 有 效 性 

在 ROs 中 ， 一 个 有 效 的 资源 名 称 应 该 具有 如 下 特征 。 

“ 第 一 个 字符 是 字母 、 波 浪 号 或 斜 线 。 

* 后 续 字符 可 以 是 字母 、 数 字 、 下 划 线 或 针线 。 

“基本 名 称 ” 中 不 能 有 波浪 号 或 斜 线 。 

3. 名 称 的 解析 

ROS 包 含 了 4 类 型 的 计算 图 资源 名 称 : 基本 名 称 、 相 对 名 称 、 全 局 名 称 和 私有 名 称 ， 其 语法 分 别 如 下 。 

- 基本 名 称 : base 

FAX LAK: relative/name 

. 全 局 名 称 : /global/name 

- 私有 名 称 : ~private/name 

默认 情况 下 ， 名 称 解 析 是 相对 命名 空间 进行 的 。 如 节点 /ur5/node1 的 命名 空间 为 /ur5， 因 此 若 该 节点 使 用 了 “res” 资源 ， 那 么 该 名 称 实际 上 被 解析 为 /ur5/res。 
没有 命名 空间 修饰 符 的 名 称 都 是 基本 名 称 。 基 本 名 称 实际 上 是 一 种 特殊 的 相对 名 称 ， 因 此 具有 相同 的 解析 规则 。 基 本 名 称 用 得 最 多 的 场合 是 用 来 初始 化 节点 名 称 。 
以 斜 线 (/) 开头 的 名 称 是 全 局 名 称 ， 全 局 名 称 是 被 完整 解析 的 。 全 局 名 称 在 一 定 程度 上 限制 了 代码 的 可 移植 性 ， 所 以 应 当 尽 量 避 免 使 用 。 


以 波浪 号 (~) 开头 的 名 称 是 私有 和 名称。 私有 名 称 把 节点 名 称 转换 为 一 个 命名 空间 。 例 如 ， 若 节点 /ur5/node1 中 使 用 了 “~res” 资 源 ， 那 么 该 名 称 实际 上 是 被 解析 为 /ur5/node1/res。 


2.2 ”ROS2.0 框 架 


2.2.1 ”ROS2.0 简 介 


由 于 ROS 已 经 存在 了 较 长 时 间 ， 已 采用 其 开发 机 器 人 系统 的 开发 者 希望 ROS 仍 然 能 保持 现在 的 应 用 状态 ， 而 不 会 被 ROS 的 不 断 发 展 所 影响 ， 所 以 ROS2.0 是 独立 于 ROS 并 行 的 包 集 ， 可 以 单独 安装 并 且 可 
以 与 ROS 进 行 交互 。 

ROS2.0 是 一 个 与 ROS 类 似 的 中 间 件 系统 ， 它 使 用 的 也 是 现成 的 开源 库 ， 这 样 开发 者 可 以 维护 更 少 的 代码 ， 尤 其 是 非 机 器 人 特定 代码 ;充分 利用 超过 开发 者 能 力 范围 的 现 有 库 ; 受益 于 其 他 开发 者 未 来 对 
这 些 库 的 改进 。ROS 与 ROS2.0 的 设计 架构 如 图 2-5 所 示 。 


usercode.cpp usercode.py 
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图 2-5 ROS 与 ROS2.0 设 计 架 构 


可 靠 通 信 、 通 信 节 点 动态 发 现 ， 并 用 共享 内 存 的 方式 使 得 通信 效率 更 高 。 通 过 使 用 


器 


ROS2.0 的 改进 主要 是 为 了 让 ROS 更 符合 工业 级 的 运行 标准 ,采用 了 DDS (数据 分 发 服务 ) 这 个 工业 级 的 中 间 件 来 负 
DDS， 所 有 节点 的 通信 拓扑 结构 都 依赖 于 动态 点 对 点 的 自发 现 模 式 ， 从 而 去 掉 了 ROS Master 这 个 中 心 节点 。 


ROS2.0 改 变 了 API。 目 前 ROS 代 码 中 大 量 的 APl 是 在 2009 年 发 布 的 ， 虽 然 很 稳定 ， 但 未 必 是 最 好 的 ， 所 以 ROS2.0 设 计 了 新 的 APl， 同 时 尽 最 大 能 力 与 第 一 代 APl 结 合 。 所 以 ROS2.0 的 关键 概念 (分布 式 
处 理 、 匿 名 的 发 布 /订阅 消息 、 带 有 反馈 的 RPC、 语 言 无 关 性 等 ) 仍然 是 与 ROS 相 同 的 ,但 ROS2.0API 与 现 有 的 ROS 代 码 并 不 兼容 ， 好 在 目前 已 有 开发 者 实现 了 ROS2.0 与 ROS 之 间 的 兼容 ， 并 在 ROS2.0 的 软 
件 包 集 下 发 布 了 一 个 ros1_bridge 包 ， 用 于 实现 ROS 与 ROS2.0 之 间 的 双向 通信 。 


目前 ，ROS2.0 已 经 发 布 了 一 部 分 功能 包 ， 世 界 各 地 的 ROS 开 发 者 还 在 不 断 地 为 它 作 贡献 ， 将 新 开发 的 功能 包 集 发 布 到 这 个 系统 中 。 
ROS2.0 是 ROS 的 功能 扩展 和 性 能 优化 ， 其 设计 目标 主要 体现 在 以 下 5 个 方面 。 

. 支持 多 机 器 人 系统 ， 包 括 不 可 靠 的 网 络 。 

“ 消除 原型 系统 和 最 终 产 品 之 间 的 差距 。 

. 可 以 运行 在 小 型 嵌入 式 平台 上 。 

“ 支持 实时 控制 。 


.支持 交 双 平台。 


2.2 ”ROS2.0 框 架 


2.2.1 ”ROS2.0 简 介 
由 于 ROS 已 经 存在 了 较 长 时 间 ， 已 采用 其 开发 机 器 人 系统 的 开发 者 希望 ROS 仍 然 能 保持 现在 的 应 用 状态 ， 而 不 会 被 ROS 的 不 断 发 展 所 影响 ， 所 以 ROS2.0 是 独立 于 ROS 并 行 的 包 集 ， 可 以 单独 安装 并 且 可 
以 与 ROS 进 行 交互 。 


ROS2.0 是 一 个 与 ROS 类 似 的 中 间 件 系统 ， 它 使 用 的 也 是 现成 的 开源 库 ， 这 样 开 发 者 可 以 维护 更 少 的 代码 ， 尤 其 是 非 机 器 人 特定 代码 ; 充分 利用 超过 开发 者 能 力 范围 的 现 有 库 ; 受益 于 其 他 开发 者 未 来 对 
这 些 库 的 改进 。ROS 与 ROS2.0 的 设计 架构 如 图 2-5 所 示 。 


usercode.cpp 


usercode.cpp sercode.« usercode.py 
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图 2-5 ”ROS 与 ROS2.0 设 计 架 构 


ROS2.0 的 改进 主要 是 为 了 让 ROS 更 符合 工业 级 的 运行 标准 ,采用 了 DDS (数据 分 发 服务 ) 这 个 工业 级 的 中 间 件 来 负 
DDS， 所 有 节点 的 通信 拓扑 结构 都 依赖 于 动态 点 对 点 的 自发 现 模 式 ， 从 而 去 掉 了 ROS Master 这 个 中 心 节点 。 


可 靠 通 信 、 通 信 节 点 动态 发 现 ， 并 用 共享 内 存 的 方式 使 得 通信 效率 更 高 。 通 过 使 用 


器 


ROS2.0 改 变 了 API。 目 前 ROS 代 码 中 大 量 的 APl 是 在 2009 年 发 布 的 ， 虽 然 很 稳定 ， 但 未 必 是 最 好 的 ， 所 以 ROS2.0 设 计 了 新 的 APl， 同 时 尽 最 大 能 力 与 第 一 代 APl 结 合 。 所 以 ROS2.0 的 关键 概念 (分布 式 
处 理 、 匿 名 的 发 布 /订阅 消息 、 带 有 反馈 的 RPC、 语 言 无 关 性 等 ) 仍然 是 与 ROS 相 同 的 ,但 ROS2.0API 与 现 有 的 ROS 代 码 并 不 兼容 ， 好 在 目前 已 有 开发 者 实现 了 ROS2.0 与 ROS 之 间 的 兼容 ， 并 在 ROS2.0 的 软 
件 包 集 下 发 布 了 一 个 ros1_bridge 包 ， 用 于 实现 ROS 与 ROS2.0 之 间 的 双向 通信 。 


目前 ，ROS2.0 已 经 发 布 了 一 部 分 功能 包 ， 世 界 各 地 的 ROS 开 发 者 还 在 不 断 地 为 它 作 贡 献 ， 将 新 开发 的 功能 包 集 发 布 到 这 个 系统 中 。 
ROS2.0 是 ROS 的 功能 扩展 和 性 能 优化 ， 其 设计 目标 主要 体现 在 以 下 5 个 方面 。 

. 支持 多 机 器 人 系统 ， 包 括 不 可 靠 的 网 络 。 

` 消除 原型 系统 和 最 终 产品 之 间 的 差距 。 

. 可 以 运行 在 小 型 赃 入 式 平台 上 。 

“ 支持 实时 控制 。 


. 支持 交叉 平台 。 


2.2.2 ROS 与 ROS2.0 之 则 的 主要 区 别 


1. 发 布 / 订 阅 


图 2-6 是 ROS2.0 实 现 发 布 /订阅 的 结构 ，ROS2.0 将 ROS 中 原来 的 通信 协议 TCPROS 和 UDPROS 蔡 换 为 DDSI-RTPS。 此 外 ，APl 也 做 了 相应 的 改变 ， 例 如， 节点 的 名 字 没 有 传递 给 全 局 的 init () 函数 ,而 
是 传递 给 节点 的 构造 函数 ，spin () 函数 不 再 是 全 局 的 ， 而 是 通过 节点 调用 等 。 


发 布 者 


rw EL rmw #20 


Connext OpenSplice Connext OpenSplice 





DDSI-RTPS 


图 2-6 ROS2.0 发 布 /订阅 

2. 进 程 内 通信 

ROS2.0 与 ROS 的 进程 内 通信 和 具有 相同 的 功能 ， 但 是 ROS2.0 采 用 了 更 加 安全 的 模式 。 

ROS2.0 的 进程 内 通信 和 具有 如 下 特点 。 

. 避免 了 序列 化 和 北 序 列 化 。 

避免 了 网 络 栈 、 将 数据 分 成 包 。 

- 通过 提供 基于 API 的 unique_ptr 可 安全 避免 复制 。 

进程 内 和 进程 间 的 通信 更 加 一 致 。 
3. 网 络 通信 
表 2-1 列 出 了 ROS 和 ROS2.0 的 一 些 QoS 设 置 对 比 。 


表 2-1 ROS 与 ROS2.0 的 一 些 QoS 设 置 对 比 





ROS ROS 2.0 
ERS Wwe UDPROS/TCPROS AY SE Fe 
消息 存储 保存 全 部 消息 
消息 保重 At ANT RAF 永久 保存 





ROS 消 息 传递 采用 尽量 到 达 ， 尽 最 大 可 能 丢失 最 少 的 消息 ， 而 ROS2.0 会 保证 所 有 的 消息 都 会 到 达 另 一 端 ，ROS 采 用 队列 存储 消息 ， 队 列 的 长 度 可 以 指定 ， 而 ROS2.0 会 存储 所 有 的 消息 ; ROS 数据 没有 
持久 性 ，ROS2.0 的 数据 会 被 发 送 者 持续 保存 。 此 外 ，ROS2.0 中 的 DDS 也 提供 了 一 些 已 被 工业 应 用 证 明 有 效 的 QoS: 使 用 UDP (代替 TCP) SMS, 这样 发 布 者 就 不 需要 为 每 一 个 订阅 者 都 复制 一 份 数据 
T; 支持 不 可 靠 网 络 ， 如 蜂窝 网 络 、loT、 高 延 时 的 连接 等 。 


4.ROS 和 ROS2.0 之 间 的 软件 桥 ( 见 图 2-7) 
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图 2-7 ROS 与 ROS2.0 之 间 的 软件 桥 
ROS 和 ROS2.0 通 过 软件 桥 (bridge) 进行 数据 通信 ， 其 实现 的 技术 背景 具体 如 下 。 
- 目前 bridge 是 使 用 C+ 十 实现 的 。 
. 消息 定义 : ROS 使 用 tosmsg API，ROS2.0 使 用 ament 资 源 索 引 。 
“ 在 ROS 和 ROS2.0 类 型 间 采 用 自动 的 映射 规则 ; 也 可 以 选择 指定 通用 的 规则 (在 .yaml 文 件 中 进行 定义 ) ; 对 于 任意 的 类 型 对 ， 可 以 注册 一 个 函数 ， 用 于 产生 ROS 和 ROS2.0 之 间 的 转换 函数 。 
. 建立 一 个 软件 桥 : ROS 使 用 toscpp， 通 过 pkg-config 实 现 ， 而 ROS2.0 使 用 tclcpp， 通 过 CMake find_package () 实现 。 
“ 具体 实现 要 求 所 有 的 头 浮 数 必 须 没有 冲突 。 
5. 基 本 开发 环境 的 区 别 
1) ROS: Linux (Ubuntu) ; OS X。 
2) ROS2.0: Linux (Ubuntu) ; OS X; Windows, 


从 上 述 所 列 开发 环境 可 以 看 出 ，ROS2.0 扩 展 了 Windows 基 础 平台 ， 使 其 具有 更 广泛 的 适应 性 。 


2.3 ABs 


本 章 首先 介绍 了 ROS 的 基本 发 展 情况 和 主要 特点 ， 然 后 对 ROS 整 体 架构 从 文件 系统 级 、 计 算 图 级 、 开 源 社 区 级 分 别 进行 了 论述 ， 其 中 针对 计算 图 级 的 基本 要 素 ， 如 节点 、 主 题 、 服 务 、 消 息 等 进行 了 详 
细 说 明 。 此 外 ， 当 构建 一 个 复杂 的 大 系统 时 ， 名 称 系统 尤为 重要 ， 所 以 本 章 也 对 名 称 系统 进行 了 详细 说 明 。 最 后 ， 介 绍 了 ROS2.0 的 基本 架构 ， 并 与 ROS 进 行 了 对 比 ， 发 现 ROS2.0 在 架构 、 通 信 方 式 等 方面 
都 进行 了 改进 ， 并 且 功 能 更 强大 、 适 用 平台 更 广泛 。 
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3.1 ”ROS 通信 机 制 概述 


3.1.1 ROS 通信 和 机制 慨 念 
ROS 的 核心 功能 是 提供 一 种 软件 点 对 点 通信 机 制 ， 基 于 这 一 机 制 ， 开 发 人 员 可 以 非常 灵活 和 高 效 地 组 织 智能 机 器 人 的 软件 实现 。ROS 的 运行 架构 是 一 种 基于 Socket 网 络 连 接 的 松 看 合 架 构 。 在 这 个 运行 
架构 中 包括 一 系列 进程 ， 这 些 进程 可 以 驻 留 在 多 个 不 同 的 主机 上 并 且 在 运行 的 过 程 中 通过 点 对 点 的 拓扑 结构 实现 通信 。 


ROS 将 每 个 工作 进程 看 作 一 个 节点 ， 使 用 节点 管理 器 进行 统一 管理 ， 并 提供 一 套 相应 的 消息 传递 机 制 。 在 ROS 中 ， 所 有 的 消息 通信 都 必须 使 用 节点 管理 器 。ROS 的 特殊 性 主要 体现 在 消息 通信 层 ， 而 不 
是 更 深 的 层次 。 点 对 点 的 连接 和 配置 通过 XMLRPC 机 制 实 现 ， 节 点 间 的 数据 流通 过 网 络 套 接 字 实 现 ， 数 据 流 在 ROS 中 被 称 为 消息 ， 模 块 间 的 消息 传递 采用 简单 的 、 语 言 无 关 的 接口 定义 描述 。 


ROS 底 层 的 通信 和 是 通过 HTTP 完 成 的 ， 因 此 ROS 内 核 本 质 上 是 一 个 HTTP 服 务 器 ， 它 的 地 址 一 般 是 http://localhost: 11311/， 即 本 机 的 11311 端 口 ， 当 需要 连接 到 另 一 台 计 算 机 上 运行 的 ROs 时 ， 只 要 连 
上 该 机 的 11311 端 口 即 可 。 
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3.1.2 ”ROS 通 信和 机 制 的 基本 要 素 

ROS 通 信 机 制 的 基本 要 素 主要 是 第 2 章 “ 计 算 图 级 ”介绍 中 给 出 的 概念 ， 包 括 节 点 、 节 点 管理 器 、 参 数 服务 器 、 消 息 、 服 务 、 主 题 ， 这 些 要 素 在 介绍 ROS 通 信 机 制 中 反复 出 现 ， 为 了 便于 理解 ， 下 面 简要 
给 出 有 关 定 义 。 

“节点: 节点 是 主要 的 计算 执行 进程 。 

` 节点 管理 器 : 节点 管理 器 的 主要 目的 是 实现 节点 之 间 的 通信 ， 用 于 主题 、 服 务 的 注册 和 查找 等 ， 参 数 服务 器 也 运行 在 节点 管理 器 之 上 。 

. 参数 服务 器 : 用 于 存储 和 检索 运行 时 参数 。 

` 消息 : 节点 之 间 的 通信 是 通过 消息 传递 来 实现 的 。ROS 中 包含 多 种 标准 类 型 的 消息 ， 同 时 编程 人 员 也 可 以 自 定义 消息 类 型 。 

. 主题 : 发布 订阅 采用 主题 实现 消息 通信 ， 每 一 条 消息 都 要 发 布 到 相应 的 主题 中 。 通 过 主题 实现 发 布 者 和 订阅 者 之 间 的 解 辜 ， 主 题 的 名 称 必须 是 独一无二 的 。 


* 服务 : 请 求 应 答 模型 使 用 服务 进行 通信 ， 提 供 一 对 一 的 连接 。 服 务 也 必须 有 一 个 唯一 的 名 称 。 当 一 个 节点 提供 菜 个 服务 时 ， 所 有 的 节点 都 可 以 通过 使 用 ROS 客 户 端 库 所 编写 的 代码 与 它 进行 通信 。 


ROS 的 运行 架构 是 一 种 使 用 ROS 通 信和 实现 节 点 间 点 对 点 的 松 厢 合 网 络 连接 的 处 理 架 构 ， 通 过 提供 多 种 类 型 通信 机 制 ， 包 括 基 于 主题 的 异步 数据 流通 信 、 基 于 服务 的 同步 RPC (远程 过 程 调用 ) 通信 及 基 
于 参数 服务 器 的 数据 传递 ， 支 持 实现 不 同 需求 的 网 络 连 接 。 虽 然 ROS 通 信 机 制 目前 不 具有 严格 的 实时 性 ， 但 是 也 能 够 支撑 不 少 类 别 的 智能 机 器 人 开发 。 


基于 主题 的 异步 数据 流通 信和 是 ROS 中 最 主要 的 通信 方式 ， 它 实现 了 节点 间 多 对 多 的 连接 ， 并 且 采 用 单 向 数据 流传 送 数 据 。 通 过 主题 可 以 实现 发 布 者 和 订阅 者 之 间 的 解 厢 ， 因 此 ， 主 题 可 以 关联 任意 的 发 
布 者 和 订阅 者 。 


基于 服务 的 同步 RPC 通 信 实 现 一 对 一 的 连接 ， 采 用 请 求 /应 答 模型 。 当 一 个 节点 需要 向 另 一 个 节点 发 送 数 据 并 要 从 另 一 个 节点 中 得 到 响应 时 ， 就 需要 使 用 该 通信 方式 。 


参数 服务 器 是 一 个 可 以 通过 网 络 访问 的 多 元 共享 的 参数 字典 ， 节 点 在 运行 时 可 以 在 参数 服务 器 上 存储 或 获取 参数 。 参 数 服 务 器 运行 在 节点 管理 器 上 ， 它 的 应 用 程序 编程 接口 通过 通用 XMLPRC 库 进行 访 
问 。 参 数 服务 器 主要 用 来 实现 一 些 参 数 配 置 等 。 


ROS 针 对 实时 性 要 求 高 的 场景 还 提供 了 一 种 通信 方式 一 一 actionlib， 它 为 优先 级 任务 提供 了 标准 的 接口 。actionlib 与 服务 通信 类 似 ， 不 同 的 是 它 是 可 剥夺 的 。 在 一 些 情况 下 ， 节 点 请 求 的 某 些 任务 需要 
长 时 间 执 行 ， 并 且 希 望 可 以 在 请 求 执行 过 程 中 中 断 任务 的 执行 ,或 者 在 任务 执行 过 程 中 获得 周期 性 的 状态 有 反馈。 因此，ROS 提 供 了 可 剥夺 任务 调度 接口 actionlib， 该 接口 提供 了 服务 节点 在 执行 耗 时 任务 时 
的 中 断 能 力 和 状态 跟踪 能 力 。 


3.2 ”基于 主题 的 异步 数据 流通 信 
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主题 用 于 发 布 订阅 模型 的 消息 传递 ， 它 拥有 独一无二 的 名 字 。 在 智能 机 器 人 系统 中 ， 若 一 个 节点 需要 与 其 他 节点 进行 通信 ， 则 只 需要 在 自己 关心 的 主题 上 发 布 或 订阅 消息 即 可 ， 发 布 者 和 订阅 者 无 需 知 
晓 彼此 的 存在 ， 从 而 实现 了 发 布 者 与 订阅 者 之 间 的 解 厢 。 发 布 者 、 订 阅 者 和 主题 之 间 的 关系 如 图 3-1 所 示 。 
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图 3-1 发 布 者 、 订 阅 者 和 主题 之 间 的 关系 


如 图 3-1 所 示 ， 在 ROS 中 ， 一 个 主题 上 可 以 拥有 多 个 发 布 者 和 订阅 者 ， 同 时 一 个 节点 可 以 发 布 或 订阅 多 个 主题 ， 并 且 可 以 随时 创建 发 布 者 或 订阅 者 。 但 是 每 个 发 布 者 只 能 向 一 个 主题 发 布 消息 ， 每 个 订阅 
者 只 能 订阅 一 个 主题 。 


ROS 通 过 主题 实现 的 节点 间 交 换 数 据 为 单 向 数据 流 ， 发 布 者 发 布 的 消息 通过 数据 流 的 方式 传递 给 订阅 者 ， 订 阅 者 不 需要 反馈 给 发 布 者 是 否 收 到 消息 。ROS 的 主题 通信 支持 TCPROS 和 UDPROS 两 种 通信 
协议 ，TCPROS 采 用 标准 的 TCP/IP 套 接 字 ， 这 是 ROSs 默 认 的 传输 方式 ，UDPROS 采 用 标准 的 UDP 传输 方式 ， 它 是 一 种 低 延 时 、 高 效率 的 传输 方式 ， 但 可 能 产生 数据 丢失 ， 所 以 它 比较 适用 于 远程 操控 任务 。 


在 一 个 系统 中 ， 必 须 包含 一 个 节点 管理 器 Master。Master 用 来 管理 帮 布 者 和 订阅 者 ， 使 得 发 布 者 发 布 的 消息 可 以 正确 地 发 送 给 订阅 者 ， 因 此 每 当 有 一 个 新 的 发 布 者 或 订阅 者 产生 时 ， 都 必须 在 Master 
上 注册 信息 。 当 发 布 者 在 Master 上 注册 时 ，Master 会 保存 发 布 者 的 URI (统一 资源 标识 符 ) 和 发 布 者 发 布 的 主题 ; 当 系 统 中 有 一 个 新 的 订阅 者 在 Master 上 注册 时 ，Master 会 根据 订阅 者 订阅 的 主题 ， 在 保 
存 的 信息 中 寻找 与 主题 匹配 的 发 布 者 ， 然 后 将 这 些 发 布 者 的 URI 帮 送 给 订阅 者 。 订 阅 者 会 根据 发 布 者 的 URI 与 这 些 发 布 者 建立 一 对 多 的 连接 ， 从 而 接收 这 些 发 布 者 在 主题 上 发 布 的 消息 。 


基于 主题 的 数据 流通 信和 主要 通过 以 下 4 步 来 完成 ， 如 图 3-2 所 示 。 

1) 通过 XMLRPC， 发 布 者 与 订阅 者 在 Master 上 注册 ， 订 阅 者 与 发 布 者 协商 共同 支持 的 协议 (TCPROS 或 UDPROS) 。 

2) 发 布 者 和 订阅 者 通过 互相 发 送 header， 建 立 TCPROS/UDPROS 连 接 。 

3) 经 过 以 上 两 步 ， 连 接 已 经 建立 成 功 ， 可 以 进行 数据 传送 。 发 布 者 向 特定 的 主题 发 布 消息 ， 订 阅 者 会 在 特定 的 主题 上 接收 消息 ， 然 后 将 消息 保存 在 回调 函数 队列 中 ， 等 待 处 理 。 


4) 最 后 调用 回调 函数 队列 中 已 经 注册 的 回调 函数 来 处 理 接收 到 的 消息 。 


3.2 ”基于 主题 的 异步 数据 流通 信 


3.2.1 简介 


题 用 于 发 布 订阅 模型 的 消息 传递 ， 它 拥有 独一无二 的 名 字 。 在 智能 机 器 人 系统 中 ， 若 一 个 节点 需要 与 其 他 节点 进行 通信 ， 则 只 需要 在 自己 关心 的 主题 上 发 布 或 订阅 消息 即 可 ， 发 布 者 和 订阅 者 无 需 知 
晓 彼此 的 存在 ， 从 而 实现 了 发 布 者 与 订阅 者 之 间 的 解 看。 发 布 者 、 订 阅 者 和 主题 之 间 的 关系 如 图 3-1 所 示 。 
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图 3-1 发布 者 、 订 阅 者 和 主题 之 间 的 关系 


如 图 3-1 所 示 ， 在 ROS 中 ， 一 个 主题 上 可 以 拥有 多 个 发 布 者 和 订阅 者 ， 同 时 一 个 节点 可 以 发 布 或 订阅 多 个 主题 ， 并 且 可 以 随时 创建 发 布 者 或 订阅 者 。 但 是 每 个 发 布 者 只 能 向 一 个 主题 发 布 消息 ， 每 个 订阅 


者 只 能 订阅 一 个 主题 。 
ROS 通 过 主题 实现 的 节点 间 交 换 数 据 为 单 向 数据 流 ， 发 布 者 发 布 的 消息 通过 数据 流 的 方式 传递 给 订阅 者 ， 订 阅 者 不 需要 反馈 给 发 布 者 是 否 收 到 消息 。ROS 的 主题 通信 支持 TCPROS 和 UDPROS 两 种 通信 
协议 ，TCPROS 采 用 标准 的 TCP/IP 套 接 字 ， 这 是 ROS 默 认 的 传输 方式 ; UDPROS 采 用 标准 的 UDP 传 输 方 式 ， 它 是 一 种 低 延 时 、 高 效率 的 传输 方式 ， 但 可 能 产生 数据 丢失 ， 所 以 它 比 较 适 用 于 远程 操控 任务 。 


在 一 个 系统 中 ， 必 须 包含 一 个 节点 管理 器 Master。Master 用 来 管理 帮 布 者 和 订阅 者 ， 使 得 发 布 者 发 布 的 消息 可 以 正确 地 发 送 给 订阅 者 ， 因 此 每 当 有 一 个 新 的 发 布 者 或 订阅 者 产生 时 ， 都 必须 在 Master 
上 注册 信息 。 当 发 布 者 在 Master 上 注册 时 ，Master 会 保存 发 布 者 的 URI (统一 资源 标识 符 ) 和 发 布 者 发 布 的 主题 ; 当 系 统 中 有 一 个 新 的 订阅 者 在 Master 上 注册 时 ，Master 会 根据 订阅 者 订阅 的 主题 ， 在 保 
存 的 信息 中 寻找 与 主题 匹配 的 发 布 者 ， 然 后 将 这 些 发 布 者 的 URI 帮 送 给 订阅 者 。 订 阅 者 会 根据 发 布 者 的 URI 与 这 些 发 布 者 建立 一 对 多 的 连接 ， 从 而 接收 这 些 发 布 者 在 主题 上 发 布 的 消息 。 

基于 主题 的 数据 流通 信 主 要 通过 以 下 4 步 来 完成 ， 如 图 3-2 所 示 。 

1) 通过 XMLRPC， 发 布 者 与 订阅 者 在 Master 上 注册 ， 订 阅 者 与 发 布 者 协商 共同 支持 的 协议 (TCPROS 或 UDPROS) 。 

2) 发 布 者 和 订阅 者 通过 互相 发 送 header， 建 立 TCPROS/UDPROS 连 接 。 


连接 已 经 建立 成 功 ， 可 以 进行 数据 传送 。 发 布 者 向 特定 的 主题 发 布 消息 ， 订 阅 者 会 在 特定 的 主题 上 接收 消息 ， 然 后 将 消息 保存 在 回调 函数 队列 中 ， 等 待 处 理 。 


3) 经 过 以 上 两 步 ， 
4) 最 后 调用 回调 函数 队列 中 已 经 注册 的 回调 函数 来 处 理 接收 到 的 消息 。 


3.2.2 ”异步 数据 流 的 实现 基础 


1. 节 点 准备 
(1) 初始 化 


在 创建 每 个 节点 时 ， 首 先 调用 ros: : init () 完成 对 节点 的 初始 化 。ros: : init () 主要 完成 以 下 操作 。 









|' apon -—| XMLRPC 
x i \ 

3) connect (“cacn”, TCP) 
AY 


4) TCP server:f002345 


` 





MLRPC foo: ] 234} 
5) connect (foo:2345) 


TCP data:fo00:2345 | 
6) data meddages TCP 





: this node, tos: : 
: : this node: : 


图 3-2 ”基于 主题 的 数据 流通 信 过 程 
EMAL SK: 当主 函数 执行 完毕 时 ， 通 过 已 经 注册 的 终止 函数 检查 节点 是 否 已 经 正常 关闭 ， 如 果 没 有 正常 关闭 则 调用 tos: : shutdown () 关闭 节点 。 
建立 全 局 回调 函数 队列 : 在 节点 中 默认 使 用 全 局 回调 函数 队列 ， 用 于 存储 用 户 编写 的 回调 函数 和 待 处 理 的 消息 。 
file_ log、tos: : patam 初 始 化 函数 实现 一 些 全 局 变量 的 初始 化 。ros: : network: : init () 完成 对 节点 主机 名 或 IP 
init () 完成 对 节点 命名 空间 及 名 字 的 初始 化 ; ros: : fle log: : init () 完成 


: network, tos: : master, fos: 
init () 获取 Mastet 的 URI、 主 机 号 及 端口 号 ; ros 


全 局 变量 初始 化 : 通过 调用 tos: 
地 址 ， 以 及 TCP 连 接 中 监听 端口 的 初始 化 ; tos: : master: : 
日 志文 件 路 径 的 初始 化 ; rosi : param: : init O 主要 在 参数 服务 器 上 存储 一 些 参数 
- 创建 通信 接口 : NodeHandle 类 创建 发 布 者 、 订 阅 者 、 服 务 客户 端 等 的 roscpp 接 口 ， 它 提供 RAIT (Resource Acquire Is Initial ， 资 源 分 配 即 初始 化 ) 接口 ， 当 创建 第 一 个 NodeHandle 对 象 时 ， 实 例 化 这 个 节 


点 所 必需 的 所 有 内 容 ， 当 节点 中 最 后 一 个 NodeHandle 被 销毁 时 ， 才 会 关闭 这 个 节点 中 的 所 有 实例 化 内 容 。 
start () ， 实 例 化 TopicManager、ServiceManager、Connection- 


(2) 启动 管理 器 
在 节点 完成 初始 化 之 后 ， 可 以 创建 ros: : NodeHandle 类 对 象 。 当 节点 创建 第 一 个 Node-Handle 对 象 时 ， 调 用 ros: 
Manager、PollIManager 和 XMLRPCManager 类 ， 并 在 所 有 实例 上 调用 它们 的 成 员 函 数 start () ， 使 得 它们 执行 相应 的 操作 ， 图 3-3 展 示 了 它们 的 主要 操作 。 


ros::start( ) 





XMLRPCManager::srrverlhreadFunc ; 
= f PollManager::threadFunc 


server .bindAndListen(0) 
建立 需要 的 XMLRPC 连 接 产生 信和 号， 触发 所 村 
删除 要 断 开 的 连接 处 理 poll_set 上 的 事件 
TCP 在 全 局 关口 上 监听 ，UDP 在 关口 0 
上 监听 ， 会 在 poll set 上 产生 事件 


在 信号 poll_signal 上 添加 插 情 


TopicManager::processPublisherQueues() 


在 信号 poll singnal 上 添加 本 楼 
ConnectionManager::removeDropedC onnections() 


图 3-3 ros: : start () 的 具体 实现 


XMLRPCManager 定 义 了 一 个 XMLRPC 服 务 器 server _， 并 在 XMLRPC 服 务 器 端口 0 上 监听 ， 同 时 创建 serverThreadFunc 线 程 ， 该 线程 用 于 创建 XMLRPC 连 接 ， 断 开 不 需要 的 XMLRPC 连 接 。 
XMLRPCManager 用 于 节点 间 、 节 点 与 节点 管理 器 之 间 通 过 XMLRPC 建 立 连 接 。 


PollManager 用 于 节点 内 / 间 的 数据 流通 信 ， 监 控 所 有 的 非 XMLRPC 的 网 络 事件 ， 组 件 在 它 上 面 注册 文件 描述 符 ， 然 后 调用 系统 调用 poll () 监控 它们 。PollManager 中 定义 了 一 个 PollSet 类 对 象 
poll_set ， 用 于 接收 TCP 全 局 端口 、UDP0 端 口上 的 事件 ， 该 事件 会 触发 插 槽 函数 调用 ， 建 立 节点 间 TCPROS/UDPROS 连 接 ， 插 槽 函数 由 线程 thread-Func () 运行 ， 该 线程 会 一 直 存 在 ， 直 到 节点 的 最 后 
一 个 NodeHandle 被 销毁 。 还 定义 了 一 个 信号 poll_signal _， 其 上 的 插 模 函数 processPublisherQueues () 将 发 布 者 发 布 的 进程 间 消息 发 送 给 订阅 者 ;removeDropedConnections () 将 断 开 的 
TCPROS/UDPROS 连 接 删 除 。 


如 图 3-4 所 示 ， 每 个 节点 的 TopicManager 实 例 维护 一 个 Publication 列 表 和 一 个 Subs-cription 列 表 ， 列 表 中 的 每 一 个 Publication 和 Subscription 对 象 唯一 对 应 一 个 主题 。 对 于 每 个 publication， 对 应 多 
个 publisher (发 布 者 ) 和 subscriberlink (与 发 布 者 建立 连接 的 对 象 ) 。 对 于 每 个 Subscription， 对 应 多 个 subsciber (订阅 者 ) 和 publisherlink (与 订阅 者 建立 连接 的 对 象 ) ， 此 外 还 包含 一 个 
pending_connections， 和 存储 待 处 理 的 XMLRPC 连 接 。 

2. 订 阅 者 发 现 发 布 者 

(1) XMLRPC 简 介 
ROS 节 点 之 间 建 立 连接 过 程 及 其 与 Master 之 间 通 信 均 使 用 XMLRPC (基于 XML 的 远程 过 程 调 用 ) ， 这 些 远程 过 程 调 用 采用 HTTP 传 输 协 议 ，XML 作 为 传输 数据 的 编码 格式 。XML 的 文本 属性 使 得 它 独 立 


于 传输 层 的 编码 形式 ， 直 接 封装 在 HTTP 中 传输 。 远 程 过 程 调用 的 主要 作用 是 负责 节点 对 计算 图 级 中 的 信息 进行 获取 、 更 改 及 一 些 全 局 变量 的 设置 ， 不 直接 支持 数据 的 流传 输 。 现 有 的 许多 语言 都 支持 
XMLRPC， 而 且 XMLRPKC 的 调用 是 无 状态 的 ， 因 此 无 需 跟 踪 状 态 信息 ， 该 性 质 可 以 简化 实现 逻辑 。 
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图 3-4 节点 中 各 元 素 关 系 图 (THR Ftopic) 


XMLRPC 调 用 的 执行 由 请 求 和 应 答 两 个 阶段 组 成 。XMLRPC 客 户 端 将 方法 调用 请 求 发 送 给 XMLRPC 服 务 器 ， 服 务 器 会 调用 请 求 对 应 的 方法 ， 然 后 将 调用 状态 (方法 是 否 调用 成 功 ) 及 方法 返回 值 回 


客户 端 。 
(2) 数据 结构 
在 XMLRPC 语 言 中 ， 一 个 请 求 用 methodCall 实 体 引 入 ， 请 求 的 方法 名 称 用 method-Name 字 符 串 指定 ， 所 需 传递 的 参数 用 params 实 体 表 示 。params 与 函数 所 需要 的 参数 相 匹配 ， 通 常 第 一 个 参数 是 调 
用 节点 名 。params 实 体 下 有 一 组 param， 分 别 用 来 表示 每 一 个 要 传递 的 参数 。 参 数值 用 value 实 体 表示 。 利 用 XMLRPC 实 现 getPid () 调用 的 请 求 在 HTTP 上 传输 的 内 容 如 图 3-5 所 示 。 
响应 包含 方法 调用 的 状态 和 返回 值 。ROS 的 XMLRPC 返 回 值 为 一 个 包含 三 部 分 的 数组 ， 第 一 部 分 是 状态 编码 ， 是 一 个 整数 ， 只 能 是 以 下 三 种 值 。 


客户 端 有 一 个 错误 ， 方 法 将 不 会 被 执行 。 例 如 ， 错 误 的 方法 名 称 、 参 数 或 类 型 错误 。 


“ -1: 
` 0: 服务 器 在 执行 方法 时 发 生 错 误 。 


.1: 调用 成 功 。 


| POST /RPC2 HTTP/1.1 

2 Content-Type: text/xml 

3 Content-Length: 178 

4 

5 <?xml version="1.0"?> 

6 <methodCall> 
<methodName>getPid</methodName> 
<params> 

<param> 


<value><string>/rosnode</string></value> 


</param> 
</params=> 
13 </methodCall> 





图 3-5 getPid () XMLRPC 调 用 请 求 在 HTTP 上 传输 的 内 容 
第 二 部 分 为 状态 字符 串 ， 是 一 个 可 读 的 文本 ， 用 来 解释 状态 编码 产生 的 原因 。 这 部 分 是 可 选 的 ， 可 以 为 空 。 
第 三 部 分 为 返回 值 ， 包 含 真正 的 通过 远程 方法 调用 产生 的 返回 值 。 它 的 类 型 与 所 调用 的 函数 返回 值 类 型 一 致 。 


利用 XMLRPC 实 现 getPid () 调用 的 响应 在 HTTP 上 传输 的 内 容 如 图 3-6 所 示 。 


1 HTTP/1.1 200 OK 

2 Content-Type: text/xml 
3 Content-Length: 309 

A 

5 <?xml version="1.0"?> 
6 <methodResponse> 


7 <params> 

8 <param> 

9 <value> 
10 <array> 
11 <data> 
1 < <vatue><i4>1</i4></vatue> 
13 <valuex><value> 
14 <value><i4>6544</i14></value> 
15 </data> 

16 </array> 

Be </value> 


18 </param> 
19 < ip arams> 
20 </methodResponse> 


图 3-6 getPid () XMLRPC 调 用 响应 在 HTTP 上 传输 的 内 容 
(3) 具体 实现 过 程 


通过 XMLRPC， 订 阅 者 可 发 现 发 布 者 ， 并 与 发 布 者 协商 共同 支持 的 协议 (TCPROS/UDPROS) ， 过 程 如 下 。 


2) 运行 发 布 者 所 在 的 节点 ， 发 布 者 在 Master 上 注册 要 发 布 的 主题 ，Master 保 存 发 布 者 所 在 节点 的 URI 和 发 布 者 发 布 的 主题 ， 如 图 3-7 所 示 。 








图 3-7 ROS RPC 连 接 建立 过 程 (一 ) 


3) 运行 订阅 者 所 在 的 节点 ， 订 阅 者 首先 在 Master 上 注册 要 订阅 的 主题 ， 之 后 Master 会 查询 保存 的 发 布 者 ， 将 主题 匹配 的 发 布 者 的 URI 发 送 给 订阅 者 ， 如 图 3-8 所 示 。 





paylocad 


TopicManager::sbscribe () 
转换 为 pub_uris 


TopicManager::registerSubscriber () 





布 者 bala ) 
Se = A ‘ it Subscription::pubUpdate (pub_uris) 


图 3-8 ROS RPC 连 接 建立 过 程 (二 ) 


4) 订阅 者 根据 Master 提 供 的 发 布 者 所 在 节点 的 URI， 与 发 布 者 协商 共同 支持 的 协议 ， 即 TCPROS 或 UDPROS， 如 图 3-9 所 示 。 


a 


pr 





PERS Subscription: :negotiateConnection () =e 


图 3-9 ROS RPC 连 接 建 立 过 程 (=) 
3.TCPROS/UDPROS 连 接 建立 
在 建 YTCPROS/UDPROS 连 接 的 过 程 中 会 创建 三 个 层次 的 类 对 象 ， 以 建立 和 维护 发 布 者 与 订阅 者 之 间 的 连接 ,分别 如 下 。 
- TransportTCP/TransportUDP 对 象 : 用 来 实现 网 络 套 接 字 通 信 中 的 数据 流 读 
- Connections} #: 其 封装 了 Transport 对 象 : 用 于 实现 消息 读 写 。 
- TransportSubsctiberLink 和 TransportPublisherLink 对 象 : 封装 了 Connection 对 象 ， 分 别 用 来 与 发 布 者 和 订阅 者 建立 连接 。 
对 于 建立 TCPROS/UDPROS 连 接 ， 客 户 端 与 服务 器 所 用 到 的 主要 类 如 表 3-1 所 示 。 


表 3-1 客户 端 与 服务 器 所 用 的 类 


= P im ARS 88 
(订阅 者 (发布 才 ) 
Subscription Publication 
Subscriber Publisher 
PublisherLink SubscriberLink 


(1) 建立 TransportTCP 对 象 层 连 接 


当 建 YXMLRPC 连 接 之 后 ， 订 阅 者 已 经 明确 了 要 连接 的 发 布 者 所 在 节点 的 URI， 并 且 明 确 了 共同 支持 的 协议 ， 则 将 已 建立 的 XMLRPC 连 接 从 待 连 接 列表 中 删除 ， 同 时 为 建立 节点 间 的 TCPROS/UDPROS 
连接 做 准备 。 以 下 给 出 的 是 建立 TCPROS 连 接 的 过 程 ，UDPROS 与 此 类 似 。 
客户 端 首先 创建 一 个 TransportTCP 对 象 ， 并 在 该 对 象 上 调用 connect () 函数 ， 准 备 连 接 到 远程 服务 器 上 。 在 服务 器 端 ， 节 点 在 创建 第 一 个 Nodehandle 时 ， 已 经 通过 Connection-Manager: 


start () 函数 创建 了 一 个 TransportTCP 对 象 ， 并 且 已 经 在 该 对 象 上 调用 了 listen () 函数 ， 使 得 该 对 象 在 服务 器 节点 的 全 局 TCP 端 口上 进行 监听 。 上 此外， 服务 器 节点 在 创建 第 一 个 Nodehandle 时 通过 
PollManager 创 建 的 线程 会 处 理 套 接 字 端 口 监听 到 的 事件 ， 从 而 导致 服务 器 节点 的 TransportTCP: : accept () 函数 被 调用 ， 实 现 客户 端的 连接 请 求 处 理 。 图 3-10 展 示 了 TransportTCP 层 的 连接 过 程 。 


connectbH 3E 


connecti/Z |e] 
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图 3-10 ”TransportTCP 连 接 的 建立 过 程 


TransportTCP 对 象 层 的 连接 实际 是 通过 调用 Socket 的 编程 接口 实现 的 。Socket 建 立 TCP 连 接 的 冰 数 与 roscpp 建 立 TransportTCP 层 连接 的 TransportTCP 类 函数 对 应 关系 如 表 3-2 所 示 。 


表 3-2 Socket 与 TranspottTCP 类 建立 TCP 连 接 函 数 对 应 关系 


Socket 函数 接口 TransportTCP 类 
socket() 
connect() 

connect) 

socket() 

bind() listen() 
listen() 

accept() accept() 


(2) 建立 Connection 和 TransportLink 对 象 层 连 接 
- headet 数 据 结 构 


在 建立 TransportTCP 对 象 层 连接 的 过 程 中 ， 发 布 者 与 订阅 者 之 间 会 互相 发 送 header， 以 建立 Connection 对 象 层 的 连接 。 建 立 Connection 对 象 层 的 连接 时 ， 订 阅 者 首先 会 给 发 布 者 发 送 一 个 header， 
之 后 发 布 者 也 会 向 订阅 者 发 送 相应 的 header， 从 而 建立 Connection 对 象 层 连接 。 发 布 者 发 送 header 的 数据 结构 如 表 3-3 所 示 。 


表 3-3 发布 者 headet 


名 称 
md5sum 消息 类 型 的 md5 Pe Sr (A 
callerid 发 布 者 所 在 节点 名 称 
latching 值 为 1， 表示 发 布 者 正在 发 送 锁定 的 消息 





订阅 者 发 送 header 的 数据 结构 如 表 3-4 所 示 。 


表 3-4 订阅 者 headet 


名 称 





message definition 消 上 县 定 父 的 完整 廊 本 是 
callerid 订阅 者 所 在 三 点 的 和 名称 是 
topic 汀 阅 者 订阅 主题 的 名 称 是 
md5sum EHA PRAY mdShash 检验 全 H 
type 消息 类 型 a 
tcp nodelay 值 为 1， 表示 在 套 接 字 上 放置 了 TCP_NODELAY 否 


为 了 保证 两 端 传输 数据 类 型 一 致 ， 会 给 出 md5 校 验 值 ， 每 次 生成 新 的 msg 时 ，md5 校 验 值 都 会 根据 内 部 数据 类 型 的 变动 而 改变 ， 从 而 避免 了 新 msg 与 上 昌 msg 传 输 类 型 不 一 致 的 问题 。 


- 具体 实现 过 程 


订阅 者 首先 创建 一 个 Connection 对 象 ， 然 后 在 该 对 象 上 调用 成 员 函 数 initialize () ， 阻 塞 等 待 ， 准 备 读 取 来 自发 布 者 的 header; 之 后 再 创建 一 个 TransportPublisherLink 对 象 ， 在 该 对 象 上 调用 
initialize () 函数 ， 该 函数 在 Connection 对 象 上 添加 一 个 回调 函数 ， 当 Connection 接 收 到 header 时 会 触发 该 回调 函数 调用 ， 从 而 利用 接收 到 的 header 初 始 化 TransportPublisherLink 中 的 变量 ; 之 后 ， 再 
创建 一 个 header， 并 调用 Connection 对 象 的 writeHeader () 函数 帮 送 给 发 布 者 。 


发 布 者 在 调用 TranportTCP 的 成 员 遂 数 Listen () 时 ， 设 置 回 调 函 数 ， 该 回调 函数 在 TransportTCP 对 象 层 客户 端 连 接 建 立成 功 时 触发 ， 创 建 一 个 Connection 对 象 ， 同 样 ， 在 该 对 象 上 调用 
Connection: : initialize () 函数 ， 阻 塞 等 待 ， 准 备 读 取 来 自 订 阅 者 的 header; 当 发 布 者 接收 到 header 时 ， 会 触发 ConnectionManager: : onConnectionHeaderReceived () 回调 函数 ， 创 建 一 个 


TransportSubscriberLink 对 象 ， 首 先 在 该 对 象 上 调用 initialize () 函数 ， 用 Connection 初 始 化 该 对 象 中 的 成 员 变量 ; 之 后 该 对 象 将 会 根据 接收 到 的 header 设 置 TransportSubscriberLink 成 员 变 量 的 值 ， 并 
发 送 一 个 header 给 订阅 者 。 


通过 以 上 过 程 ， 发 布 者 和 订阅 者 之 间 通 过 互相 发 送 header， 初 始 化 Connection 对 象 和 TransportLink 对 象 ， 从 而 建立 Connection 对 象 层 和 TransportLink 对 象 层 的 连接 ， 具 体 过 程 如 图 3-11 所 示 。 


LTB 发 布 着 


TransportPublisherLink::initalize () ~ 


Connection':initialize() 


| TransportSubscriberLink::initialize () 
Connection::initialize | 


图 3-11 Connection 和 TransportLink 对 象 层 连接 


3.2.3 ”异步 数据 流 的 实现 过 程 
1. 消 息 类 型 
发 布 者 与 订阅 者 之 间 传 递 的 消息 为 ros: : SerializedMessage 对 象 ， 在 ros: : SerializedMessage 
类 中 定义 了 以 下 成 员 变 量 ， 分 别 用 于 两 种 类 型 的 消息 传递 : 
. boost: : shated_artaybuf; 
- size_t num_bytes ; 
“ uint8_t*+message_start; 
< boost: : shared_ptrmessage ; 
< const std: : type_info*type_info ; 
基于 主题 的 异步 数据 流通 信 有 两 种 类 型 的 消息 ， 下 面 将 分 别 介绍 。 
(1) 进程 内 消息 


在 一 个 节点 内 ， 如 果 发 布 者 发 布 的 主题 与 订阅 者 订阅 的 主题 相同 ， 那 么 消息 传递 便 可 选择 进程 内 消息 传递 的 方式 。 进 程 内 消息 传递 通过 共享 指针 boost: : shared_ptr 来 完成 ， 当 发 布 者 发 布 一 个 消息 
时 ， 直 接 将 指针 传递 给 进程 内 的 订阅 者 即 可 。 进 程 内 消息 传递 可 以 省 略 序列 化 /逆序 列 化 及 节点 间 的 数据 传送 步骤 ， 从 而 减少 此 类 消息 的 延 时。 


在 这 种 消息 传递 中 使 用 ros: : SerializedMessage 的 成 员 变 量 message 和 type_info， 消 息 的 数据 存储 在 message 中 。 在 TopicManager: : publish () 函数 中 将 nocopy 的 值 设 置 为 true，serialize 的 
值 设置 为 false。 


调用 的 函数 原型 位 于 ros: : Publisher 中 ， 函 数 原型 如 下 所 示 : 





template <typename M> 
void publish (const boost::shared ptr<M>& message) const 


(2) 进程 间 消息 


当 发 布 者 与 订阅 者 不 在 同一 节点 中 时 ， 必 须 使 用 进程 间 消 息 传递 的 方式 ， 当 然 发 布 者 与 订阅 者 在 同一 节点 也 可 以 使 用 进程 间 消 息 传递 的 方式 。 发 布 者 发 布 消 息 ， 然 后 被 序列 化 ， 经 过 TCPROS/UDPROS 
网 络 传递 ， 被 订阅 者 节点 接收 ， 然 后 在 订阅 者 节点 内 进行 逆序 列 化 后 ， 传 递 给 每 一 个 订阅 该 主题 的 订阅 者 。 如 果 一 个 节点 中 存在 多 个 订阅 同一 主题 的 订阅 者 ， 那 么 节点 间 的 数据 传递 只 需要 传递 一 次 ， 订 阅 
者 节点 会 自动 将 接收 的 消息 复制 给 每 一 个 订阅 者 。 


在 这 种 消息 传递 中 使 用 ros: : SerializedMessage 的 成 员 变 量 buf、num_bytes 和 message _start 变 量 ,， 消 息 中 的 数据 存储 在 buf 中 。 在 TopicManager: : publish () 函数 中 ，nocopy 的 值 设置 为 
false，serialize 的 值 设置 为 true。 


调用 的 函数 原型 位 于 ros: : Publisher 中 ， 函 数 原型 如 下 所 示 : 


template <typename M> 
void publish(const M& message) const 





2. 进 程 间 消息 结构 


在 通过 TCPROS/UDPROS 网 络 传递 消息 时 ， 消 息 首 先 被 序列 化 为 小 端 二 进 制 模式 〈 所 有 数据 的 低 字 节 均 放 在 前 面 ) ， 当 前 广泛 应 用 的 小 端 CPU 体 系 结构 (〈 低 字 节 排放 在 内 存 的 低地 址 端 ) ， 如 X86 或 
ARM 体 系 结构 ， 处 理 这 种 序列 化 的 消息 比较 容易 。 因 此 ， 一 个 序列 化 的 消息 数据 流 与 序列 化 的 C 结 构 变量 非常 相似 。 原 始 的 数据 类 型 直接 与 C 类 型 相对 应 ， 如 图 3-12 所 示 。 变 长 数组 和 字符 串 (字符 数组 ) 
是 特例 ， 它 们 首先 需要 一 个 uint32 用 于 表示 数组 长 度 ， 随 后 是 序列 化 消息 。 相 应 地 ， 定 长 数组 被 当 作 规定 类 型 的 变量 序列 ， 任 何 幅 套 类 型 都 会 被 展开 成 原始 的 数据 类 型 或 数组 。 


ROS 类 型 
uint8 uint8_t 
Uint16 ULnt16_t 
uint32 uint32_t 
uint64 uint64_t 
int8_t 
int16_t 
int32_t 
int64_t 
time uros_time_t 
duration uros_time_t 
struct UrosString { size_t length; char *datap; }; 
struct UrosTcpRosArray { uint32_t length; void *entriesp; }; 
uint8_t 
char 
uint32_t 
int32_t 





图 3-12 ROS 和 C 类 型 的 映射 
在 网 络 中 传输 的 消息 采用 的 是 “length+content” 格 式 ，length 是 一 个 uint32 的 数据 ， 表 示 消 息 文本 的 长 度 ; content 为 序列 化 的 消息 。 


以 下 以 发 送 一 个 类 型 为 字符 串 的 “Hello，World! ”消息 为 例 ， 说 明 在 TCPROS/UDPROS 网 络 中 传输 的 数据 流 内 容 。 如 图 3-13 所 示 ，length 为 第 一 行 ; content 为 第 二 行 ， 因 为 是 字符 串 ， 所 以 需要 一 
个 uint32 表 示 字 符 串 的 长 度 。 数 据 流 的 偏 移 量 是 十 六 进 制 的 素 引 ， 用 粗 体 表 示 ; 实际 传送 的 消息 值 用 ASCI| 字 符 串 表示 ， 如 果 字符 串 不 能 打印 ， 则 用 十 六 进 制 的 数字 表示 。 右 边 的 注释 表明 了 数据 流 中 特定 字 
段 的 含义 。 


00010203 wuint32 message length 
11000000 17 


04 05 06 07 08 09 OA OB OC OD OE OF 10 11 12 13 14 string data 
oD 000000 Hello, world! 413 "Hello, World!" 





图 3-13 ROS 消息 数据 流 内 容 示 例 


3. 主 要 类 及 模块 间 关 系 
与 数据 传送 相关 的 类 及 其 作用 如 表 3-5 所 示 ， 这 些 类 都 在 命名 空间 ros 中 。 


表 3-5 主题 通信 数据 传递 相关 类 


类 作用 
Subscriber 管理 在 一 个 特定 主题 上 的 subscription 回调 图 数 
(ZĘ ) 
类 作用 
Subscription 管理 一 个 订阅 的 主题 
SubscriberLink HARA A AB AT AK 2G TV be 
TopicManager hy Ae A a AC AB PTA ot 9 PA HS 
Publisher ‘es PTE PERE AAS 
Publication bi 理 一 个 发 布 的 主题 
PublisherLink T E E A A T t PNA E, 





iJ E A 1 IM 建立 的 TCPROSAUDPROS 连接 中 接收 来 自发 布 者 的 消 
B, IMR E Fe 2624 Subscription 


he Ac 4p a Be AC Apt AY dE FE] 7A Sa a’ TCPROS/UDPROS i fe IK 25 VT 


TransportPublisherLlink 


TransportSubscriberLink 






BA T A 
IntraprocessPublisherLink 负责 订阅 者 从 发 布 者 接收 进程 内 消息 ， 然 后 将 它 转交 给 Subscription 
InterprocessSubscriberLink hf Ac TH a Be Ac Aft AY Ee AN ENA E A Be Ac 8 2 1) ee 
消息 传递 所 用 到 的 主要 类 以 及 它们 之 间 的 调用 关系 如 图 3-14 所 示 。 
I Any = 
回调 函数 1H D -一 TransportPublisherLink 








Subscriber i - 
Inteaprocess PublisherLink 

HEFEI] 
TCPROS/UDPROS 


InteaProcessSubscriberLink 


Pubslisher 


图 3-14 消息 传递 模块 间 调用 图 
其 中 ，lntraProcessPublisherLink 和 TransportPublisherLink 继 承 自 PublisherLink; Intra-ProcessSubscriberLink 和 TransportSubscriberLink 继 承 自 SubscriberLink。 
4. 发 布 与 订阅 处 理 
(1) 发 布 消息 


XMLRPC、TCPROS/UDPROS 连 接 建 立 之 后 ， 发 布 者 和 订阅 者 就 可 以 实现 真正 的 数据 传送 ， 具 体 过 程 如 图 3-15 所 示 。 程 序 开发 者 首先 需要 使 用 NodeHandle 对 象 创建 一 个 Publisher 对 象 ， 然 后 在 这 个 
WER LAA Republish () 实现 在 特定 主题 上 发 送 消 息 的 功能 


TopicManager 管 理 Publication 和 Subscription 对 象 。TopicManager: : publish () 函数 首先 会 判断 是 否 存在 订阅 者 ， 或 者 消息 对 应 主题 的 Publication 是 否 是 锁定 的 ， 如 果 存 在 订阅 者 ， 或 者 
Publication 是 锁定 的 ， 则 说 明和 存在 接收 消息 的 对 象 ， 此 时 会 将 所 传递 的 消息 根据 类 型 处 理 后 存放 在 SerializedMessage 对 象 中 ， 然 后 调用 Publication: : publish () 来 发 送 SerializedMessage 对 象 ; 否则 
就 不 发 送 消 息 ， 这 样 会 导致 消息 直接 被 丢弃 。 


Publishre::publish() PollManager: :threadFunc() 






TopicManager::proces 
sPublishQueue() 







TopicManager::publish() 

有 订阅 者 ， 或 者 Publication 是 锁定 的 
Publication::publish() Publicationon::price 
判 声 mm 的 









Ae Fy ssPublishQueue( ) 


ame EE. 







m.message 





m.buf 





IntraprocessSubscriber Link:: ane F = | 
publish queue .push back(m) 


enqueueMessage( ) 


IntraprocessSubscriber Link:: TE F 
publication: :enqueueMessage 
handleMessage() p | geU 


TransportSubscriberLink::enqueueMessage() 


将 m 保 存在 对 队列 中 
steatMessageWrite() 


connection ->write() 


图 3-15 ”发 布 消息 的 有 具体 实现 


Publication: : publish () 函数 判断 消息 类 型 ， 并 根据 不 同 的 类 型 做 不 同 的 处 理 。 如 果 SerializedMessage 对 象 的 message 中 有 数据 ， 则 表明 是 进程 内 的 消息 传递 ; 如 果 Serialized-Message 对 象 的 
buf 中 有 数据 ， 则 表明 是 进程 间 的 消息 传递 。 


如 果 消 息 是 进程 内 传递 ， 则 直接 在 Subscription 对 象 保存 的 SubscriberLink 对 象 列表 中 寻找 进程 内 的 SubscriberLink 对 象 ， 然 后 直接 在 该 对 象 上 调用 enqueueMessage 函 数 ， 将 消息 的 指针 传递 给 订阅 
者 ， 同 时 ， 在 该 函数 中 通过 调用 其 他 函数 实现 订阅 者 接收 消息 模块 。 


如 果 消 息 是 进程 间 传递 ， 则 将 消息 存储 在 Publication 对 象 的 成 员 变 量 publish_queue 中。 如 图 3-15 所 示 ，ros: : start () 创建 的 PollManager: : threadFunc () 线程 使 得 节点 中 的 所 有 
Publication 对 象 调用 processPublishQueue () 函数 来 处 理发 送 者 队列 ， 通 过 对 publish_queue 中 的 每 一 个 消息 调用 Publication: : enqueueMessage () 函数 将 其 消息 发 送出 去 ， 通 过 函数 层 层 调用 ， 
最 终 将 消息 写 入 TCPROS/UDPROS 连 接 上 ,订阅 者 会 读 取 来 自 TCPROS/UDPROS 连 接 上 的 消息 。 


(2) 订阅 消息 
订阅 者 接收 消息 的 具体 调用 流程 如 图 3-16 所 示 。 


对 于 进程 间 消 息 的 接收 ， 在 建立 TCPROS/UDPROSs 时 ， 当 订阅 者 接收 到 来 自发 布 者 的 header 之 后 ， 会 调用 onHeaderReceived 函 数 ， 首 先是 Connection 层 阻塞 等 待 ， 准 备 读 取 来 自发 布 者 的 消息 长 
度 ; 当 读 取 到 消息 长 度 后 ,会 进入 TransportPublisherLink 阻 塞 等 待 ， 准 备 读 取 来 自发 布 者 的 消息 内 容 ;， 当 消息 内 容 被 读 取 后 ，onMessage 函 数 将 接收 到 的 消息 转交 给 Subscription 对 象 。 


TransportPublisherLink::onHraderReceived() 


TransportPublisherLink::onMessageLength() 


TransportPublisherLink::onMessage() 


InteaProcessSubscriberLink:: 


handleMessage() 
HEFE] 





TransportPublisherLink::handleMessage() 
[FEFE] 





Subscription::handleMessage() 


W Hj Subscriptions] callbacks . YAm, 
RERE ANTEA ESE VRR AH 





图 3-16 “订阅 者 接收 消息 过 程 图 


handleMessage 函 数 将 接收 到 的 消息 转交 给 Subscription 对 象 进行 处 理 ， 并 将 接收 到 的 消息 保存 在 每 一 个 订阅 者 的 回调 函数 队列 中 。 


3.2.4 回调 函数 处 理 


因为 订阅 者 无 法 确定 消息 的 到 达 时 间 ， 所 以 利用 回调 函数 ， 把 响应 收 到 消息 事件 的 代码 放 到 回调 函数 里 。 通 过 轮转 函数 实现 回调 函数 的 调用 ， 从 而 实现 消息 处 理 。 
1. 回 调 函 数 简介 


roscpp 不 会 给 应 用 程序 指定 线程 模型 ， 因 此 ， 当 使 用 线程 负责 网 络 管理 、 调 度 等 操作 时 ， 它 将 隐藏 在 应 用 程序 之 后 。roscpp 人 允许 使 用 任意 数量 的 线程 调用 回调 函数 。 开 发 人 员 只 需要 调用 轮转 国 数 ， 就 
可 以 处 理 回调 函数 队列 ， 其 中 最 常用 的 是 ros: : spin () 。 


在 roscpp 中 ， 回 调 函 数 队 列 和 轮转 不 会 对 内 部 的 网 络 通 信 产 生 任 何 影响 ， 仪 当 出 现 用 户 编 写 的 回调 函数 时 ， 它 们 才 会 产生 影响 。 在 回调 函数 队列 上 轮转 的 速率 决定 了 处 理 回调 函数 的 速度 ， 而 处 理 回调 
函数 的 速度 和 消息 到 达 的 速度 又 决定 了 消息 是 否 被 丢掉 ， 因 此 在 回调 函数 队列 上 轮转 将 会 对 系统 产生 性 能 影响 。 


在 roscpp 的 发 布 /订阅 通信 方式 中 ， 设 置 了 三 种 形式 的 回调 函数 队列 ， 它 们 分 别 如 下 。 
. 全 局 回调 函数 队列 ， 即 节点 的 回调 函数 队列 。 

. 每 个 NodeHandle 的 回调 函数 队列 。 

-FATAR AAAA 
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数 队列 ， 则 会 将 消息 存放 在 NodeHandle 的 回调 函数 队列 中 ; 如 果 NodeHandle 也 没有 定义 回调 函数 队列 ， 那 就 只 能 将 消息 存放 在 全 局 回调 函数 队列 中 。 


2. 回 调 函 数 处 理 形式 

接收 到 的 所 有 消息 都 会 存储 在 回调 函数 队列 中 ， 处 理 回 调子 数 队列 主要 是 通过 以 下 几 种 形式 。 

(1) 单线 程 的 回调 函数 队列 轮转 

对 于 单线 程 程序 ， 在 回调 函数 队列 上 轮转 处 理 回 调子 数 有 两 种 形式 : ros: : spin () 和 ros: : spinOnce () ， 用 于 处 理 全 局 回调 函数 队列 。 


ros: : spin () 会 导致 所 有 用 户 编 写 的 回调 函数 被 调用 ， 并 且 一 旦 被 调用 将 不 会 返回 ， 直 到 节点 通过 调用 ros: : shutdown () 或 Ctrl+C 被 关闭 才 会 返回 。 使 用 ros: : spin () 时 ， 一 般 在 初始 化 时 
经 设置 好 了 所 有 消息 的 回调 函数 ， 并 且 不 需要 其 他 辅助 程序 运行 。 这 样 ， 每 次 消息 到 达 时 都 会 调用 用 户 的 回调 函数 ， 相 当 于 程序 是 消息 事件 驱动 的 。 


ros: : spinOnce () 调用 后 会 及 时 返回 ， 并 且 还 可 以 继续 执行 之 后 的 程序 。 它 会 在 某 个 时 间 段 内 及 时 调用 所 有 的 回调 函数 。 一 般 还 需要 其 他 辅助 程序 的 执行 ， 如 定时 任务 、 数 据 处 理 、 用 户 界 面 等 。 
(2) 多 线程 的 回调 函数 队列 轮转 
roscpp 内 部 还 支持 多 线程 调用 回调 函数 队列 ， 以 下 为 两 种 内 置 选项 。 


“ros: : MultiThreadedSpinner， 阻 塞 执行 轮转 ， 与 ros: : spin O 类 似 ， 执 行 后 无 须 返回 ， 直 到 节点 被 关闭 后 才 返 回 。 在 创建 这 种 类 型 的 对 象 时 ， 可 以 在 构造 函数 中 指定 线程 的 数量 ， 如 果 没 有 指定 ， 
或 者 将 数量 设置 为 0， 那 么 系统 默认 为 每 个 CPU 内 核 创 建 一 个 线程 。 


. ros: : AsyncSpinner， 没 用 使 用 阻塞 形式 ， 而 是 包含 了 staf () stop O WA, 这样， 编程 人 员 就 可 以 随时 打开 或 停止 在 回调 函数 队列 上 的 轮转 ， 此 外 当 创建 对 象 被 撤销 时 会 自动 停止 在 回调 函数 队 


列 上 的 轮转 。 


(3) 用 户 自 定义 回调 函数 队列 轮转 


是 使 用 默认 的 回调 国 数 队列 一 一 全 局 回调 函数 队列 ， 但 用 户 也 可 以 自 定义 NodeHandle 和 订阅 者 的 回调 函数 队列 ， 此 时 用 户 需要 自己 编写 


函数 调用 之 。 


用 户 可 以 通过 创建 ros: : CallbackQueue 对 象 来 创建 回调 函数 队列 。CallbackQueue 对 象 有 callAvailable () 和 callOne () 两 种 调用 方式 ，callAvailable () 调用 当前 队列 中 所 有 的 回调 函数 ， 而 
callOne () 仅 调 用 队列 中 最 早 的 回调 函数 。 此 外 ， 也 可 以 把 回调 函数 队列 指针 传递 给 ros: : AsyncSpinner 或 ros: : MultiThreadedSpinner 对 象 ， 通 过 它们 实现 回调 函数 队列 的 调用 。 


用 户 调用 NodeHandle: : subscribe () 函数 时 ， 会 创建 一 个 SubscribeOptions 对 象 ， 将 用 户 编写 的 回调 函数 保存 在 该 对 象 的 helper 变 量 中 。 此 外 ，NodeHandle: : subscribe () 函数 在 与 订阅 者 


题 匹配 的 Subscription 对 象 上 添加 了 一 个 Callbacklnfo 实 例 ， 用 于 保存 回调 函数 队列 和 用 户 编写 的 回调 函数 。 


Subscription 对 象 通过 调用 handleMessage () 函数 首先 将 接收 到 的 消息 和 用 户 编 写 的 回调 函数 helper 添 加 到 subscription_ queue 中 ， 然 后 再 将 subscription_queue 插入 到 callback queue 中 。 通 


过 调用 上 述 三 种 形式 的 轮转 函数 实现 callback_queue_ 中 回调 函数 的 调用 ， 从 而 导致 接收 到 的 消息 被 用 户 编写 的 回调 函数 处 理 。 


该 过 程 主要 用 到 的 数据 结构 如 表 3-6 所 示 。 


表 3-6 建立 回调 函数 队列 所 使 用 的 结构 体 


结构 体 定 义 


struct ROSCPP DECL SubscribeOptions{ 
gy E 


SubscriptionCallbackHelperPtr helper; 

CallbackQueuelInterface* callback queue; 

bool allow concurrent callba 

VoidConstPtr tracked obje 
tra 


TransportHints 


allbackQueueInterface* callback queue ; 
SubscriptionCallbackHelperPtr helper 
SubscriptionQueuePtr ee eee 
bool has tracked object; 


VoidConstwPtr tracked object; 


所 属 类 


ros 合作 空间 


ros::Subscription 


GF 


调用 三 种 形式 的 轮转 函数 都 会 导致 回调 函数 被 处 理 ， 下 面 就 以 ros: : spin () 调用 全 局 回调 函数 队列 为 例 ， 分 析 轮 转 导致 回调 函数 被 执行 的 过 程 。 


在 ros: : spin () 函数 中 创建 一 个 SingleThreadedspinner 对 象 ， 然 后 在 该 对 象 上 调用 spin 函 数 ， 如 果 用 户 没有 指定 回调 函数 队列 ， 则 将 默认 使 用 全 局 回调 函数 队列 ， 该 函数 在 回调 函数 队列 上 调用 


CallbackQueue: : callAvailable 函 数 ， 使 得 队列 中 的 所 有 回调 函数 均 被 调用 。 


执行 CallbackQueue: : callAvailable 函 数 ， 首 先 将 回调 函数 队列 中 的 元 素 保 存在 callback_queue 的 变量 tls 中 ， 然 后 对 tls 循环 调用 CallbackQueue: : callOneCB () 函数 ， 直 到 callbacks 中 所 有 回 


调 函 数 都 被 执行 。 


调用 CallbackQueue: : callOneCB () 函数 ， 取 出 tls 中 第 一 个 回调 函数 ， 对 该 回调 函数 调用 Callbacklnterface: : call () 方法 ， 其 实际 上 调用 的 是 SubscriptionQueue: : call () 方法 ， 因 为 在 
发 布 /订阅 通信 方式 中 ， 回 调 函 数 队 列 中 存储 的 回调 函数 实际 上 是 Subscription-Queue 类 型 对 象 。SubscriptionQueue: : call () 在 queue 中 取出 第 一 个 元 素 ， 将 deserializer 中 的 消息 逆序 列 化 ， 然 后 调 


用 helper 中 用 户 编写 的 回调 函数 处 理 消 息 。 
以 上 过 程 主要 使 用 的 数据 结构 如 表 3-7 所 示 。 


表 3-7 调用 回调 函数 队列 所 使 用 的 结构 体 


结构 体 定 义 所 属 类 
struct CallbackInfto{ 
CallbackInterfacePtr callback; 


uint64 t removal id; 


CallbackQueue 
(CallbackQueuelnterface 的 公有 派生 类 ) 


bool marked for removal; 
typedef std: :deque<CallbackiInfo> D CallbackInfo; 
D CallbackInfo callbacks ; 
struct TLS{ 

uint64 t calling in this. thread; 

D CallbackInfo callbacks; CallbackQueue 

D CallbackInfo::iterator cb it; (CallbackQueuelnterface 的 公有 小 生 尖 |) 
fr 
boost: :thread specific ptr<TLS> tls 
struct Item{ 


SubscriptionCallbackHelperPtr helper; 
= 


H 
pols 
È] 


MessageDeserializerPtr deseria 


bool has tracked object; 


VoidConstWPtr tracked_object; SubscriptionQueue 
bool nonconst need copy; (CallbackInterface HJA K JRAEZE ) 


ros::Time receipt time; 


typedef std: :deque<Item> D Item; 


通过 以 上 过 程 ， 接 收 到 的 消息 将 会 被 用 户 所 编写 的 回调 函数 处 理 。 


3.3 ”基于 服务 的 同步 RPC 通 信 


3.3.1 简介 


发 布 /订阅 模型 是 一 个 灵活 的 节点 间 交 流 方式 ， 但 是 它 属 于 多 对 多 、 单 向 传输 方式 ， 不 适合 RPC (远程 过 程 调用 ) 请 求 /应 答 模型 。 请 求 /应 答 是 分 布 式 系统 常用 的 信息 交互 方式 ， 所 以 ，ROS 提 供 了 基于 
服务 的 同步 RPC 通 信 方 式 ， 用 来 实现 RPC 请 求 /应 答 模型 。 服 务 由 一 对 消息 组 成 ， 一 个 用 于 实现 请 求 ， 另 一 个 用 来 实现 应 答 。 节 点 用 一 字符 名 定义 服务 ， 客 户 端 通 过 发 送 一 个 请 求 消息 调用 服务 ， 然 后 等 待 响 
应 。 


在 一 个 系统 中 ， 服 务 被 定义 在 文件 srv 中 ， 通 过 ROS 客 户 端 库 转换 为 源码 。 一 个 客户 端 可 以 与 服务 器 端 建立 持久 连接 ， 这 样 做 有 利于 提高 运行 时 性 能 ， 但 当 服务 器 端 发 生 改变 时 ， 系 统 的 健壮 性 就 会 较 
差 。 


与 发 布 / 订 阅 类 似 ， 基 于 服务 的 同步 RPC 通 信也 需要 通过 XMLRPC 在 Master 上 注册 服务 ， 客 户 端 节 点 通过 XMLRPC 在 Master 上 寻找 与 服务 对 应 的 服务 器 端 节 点 ， 然 后 与 其 建 YTCPROS 连 接 ， 之 后 才能 
进行 数据 流通 信 ， 如 图 3-17 所 示 。 







DAE Fas 





` 4) request data(args) 


nn 
XMLRPC fo0:1234) = 


5) reply data 





TCP data:foo:2345 | 





图 3-17 ROS 服 务 通信 方式 


与 发 布 /订阅 不 同 的 是 ， 同 步 RPC 服 务 通信 只 支持 TCPROS 协 议 ， 客 户 端 不 需要 通过 XMLRPC 与 服务 器 端 协商 共同 支持 的 协议 ;RPC 服 务 通信 数据 流 是 双向 的 ， 需 要 分 别 发 送 请 求 与 响应 消息 来 实现 数据 


通信 。 


3.3 ”基于 服务 的 同步 RPC 通 信 


3.3.1 f 


发 布 /订阅 模型 是 一 个 灵活 的 节点 间 交 流 方式 ， 但 是 它 属 于 多 对 多 、 单 向 传输 方式 ， 不 适合 RPC (远程 过 程 调用 ) 请 求 /应 答 模型 。 请 求 /应 答 是 分 布 式 系统 常用 的 信息 交互 方式 ， 所 以 ，ROS 提 供 了 基于 
服务 的 同步 RPC 通 信 方 式 ， 用 来 实现 RPC 请 求 /应 答 模型 。 服 务 由 一 对 消息 组 成 ， 一 个 用 于 实现 请 求 ， 另 一 个 用 来 实现 应 答 。 节 点 用 一 字符 名 定义 服务 ， 客 户 端 通 过 发 送 一 个 请 求 消息 调用 服务 ， 然 后 等 待 响 
应 。 


在 一 个 系统 中 ， 服 务 被 定义 在 文件 srv 中 ， 通 过 ROS 客 户 端 库 转换 为 源码 。 一 个 客户 端 可 以 与 服务 器 端 建立 持久 连接 ， 这 样 做 有 利于 提高 运行 时 性 能 ， 但 当 服 务 器 端 发 生 改 变 时 ， 系 统 的 健壮 性 就 会 较 
差 。 


与 发 布 /订阅 类 似 ， 基 于 服务 的 同步 RPC 通 信也 需要 通过 XMLRPC 在 Master 上 注册 服务 ， 客 户 端 节点 通过 XMLRPC 在 Master 上 寻找 与 服务 对 应 的 服务 器 端 节 点 ， 然 后 与 其 建立 TCPROS 连 接 ， 之 后 才能 
进行 数据 流通 信 ， 如 图 3-17 所 示 。 


与 发 布 /订阅 不 同 的 是 ， 


通信 。 






发 布 者 es 4) request data(arg: 
XMLRPC foo:1234} 
TCP data:foo:2345 | 


3.3.2 同步 RPC 通 信 的 实现 过 程 


1. 相 关 类 


在 roscpp 文 件 中 ， 与 服务 通信 相关 的 类 如 表 3-8 所 示 。 


AdvertiseServiceOptions 
Service 
ServiceCallbackHelper 


ServiceC lient 


ServiceClentLink 


ServiceClientOptions 
ServiceManager 
ServicePublication 


ServiceServer 


ServiceServerLink 


2. 客 户 端 发 现 服务 器 端 


同步 RPC 服 务 通信 只 支持 TCPROS 协 议 ， 客 户 端 不 需要 通过 XMLRPC 与 服务 器 端 协 商 共同 支持 的 协议 ; RPC 服 务 通信 数据 流 是 双向 的 


服务 器 用 来 与 客户 端 
消息 ， 回 客户 端 发 送 响应 消息 





5) 


5) reply data 





图 3-17 ROS 服 务 通信 方式 


表 3-8 ROS 服 务 通 信 相 关 的 类 


作用 
创建 一 个 ServiceServer 对 和 象 所 需 的 所 有 有 效 选 项 
查询 服务 的 信息 
调用 用 户 编 号 的 回调 限 数 
创建 客户 端 对 象 





EVERINA, POE H A iHi R 


a 
hh 
hee 

+! 
al 


作用 
创建 一 个 ServiceClient 对 象 所 需 的 所 有 有 效 选 项 
管理 ServicePublication 在 Master 上 的 注册 
害 理 一 个 广播 的 服务 
创建 服务 兹 对 象 
FA Siig FAR AR Hse E 





立 连 接 的 对 象 ， 向 服务 器 发 送 请 求 消息 ， 


BOAR H AR SF fi AY Me 7 TAS 


， 需 要 分 别 发 送 请 求 与 响应 消息 来 实现 数据 


对 于 服务 器 端 ， 在 调用 NodeHandle: : advertiseService () 时 ， 首 先 创建 一 个 Advertise-ServiceOptions 对 象 ， 用 于 存储 服务 名 称 和 用 户 编写 的 处 理 请 求 回调 国 数 ， 之 后 在 Master 上 注册 服务 ， 并 
创建 一 个 ServiceServer 对 象 。ServiceManager 用 于 管理 ServicePublication ， 当 调用 ServiceManager: : advertiseService () 时 ， 创 建 一 个 ServicePublication 对 象 。Service-Publication 对 象 用 于 管理 
服务 ， 包 含 与 服务 相关 的 名 称 、 服 务 类 型 、 用 户 编写 的 处 理 服务 请 求 的 回调 函数 、 回 调 函 数 队列 等 变量 。 在 Master 上 注册 服务 ，Master 将 会 保存 服务 名 称 、 服 务 节点 的 URI 和 服务 的 XMLRPC URI, 


对 于 客户 端 ， 当 调用 NodeHandle: : serviceClient () 时 ， 首 先 创建 一 个 ServiceClient-Options 对 象 ， 然 后 初始 化 服务 名 称 等 变量 ; 之 后 创建 一 个 ServiceClient。 客 户 端 不 需要 在 节点 管理 器 上 注 
册 ， 只 是 通过 Master 确 定 服务 的 URI。 


由 于 服务 通信 方式 仪 支持 TCPROSs 协 议 ， 所 以 客户 端 不 需要 与 服务 器 通过 XMLRPC 
协商 共同 支持 的 协议 。 
3. 建 立 TCPROS 连 接 
(1) 与 发 布 /订阅 的 异同 
建立 TCPROS 连 接 与 发 布 /订阅 的 相同 之 处 是 需要 创建 三 个 层次 的 类 对 象 去 建立 连接 。 
建立 TransportTCP 对 象 层 的 连接 与 发 布 /订阅 建立 TransportTCP 对 象 层 连接 的 过 程 完 全 相同 。 


建立 Connection 对 象 层 连接 时 ， 与 发 布 / 订 疝 建立 Connection 对 象 层 连接 的 过 程 基本 相同 ， 只 是 两 者 发 送 的 header 不 同 。 在 建立 Connection 对 象 层 连接 时 ， 客 户 端 首 先 向 服务 器 发 送 一 个 header， 其 
数据 结构 如 表 3-9 所 示 。 


表 3-9 BP header 


名 称 是 否 必须 包含 





callerid 客户 闪 名 称 是 
service A Pg HE Fe A AR A PR 是 
md5sum IA ESSAY AY md5 校 验 值 是 
type 是 
persistent 全 为 1， 表明 客户 凯 一 直 与 服务 大 冰 连 接 ， 可 以 发 送 多 个 请 求 G 


在 服务 器 端 收 到 来 自 客户 端的 header 后 ， 会 给 客户 端 再 发 送 一 个 相应 的 header， 服 务 器 header 的 数据 结构 如 表 3-10 所 示 。 
表 3-10 服务 器 header 
名 称 ew 是 否 必须 包含 
callerid 服务 器 端 名 称 是 





建立 ServiceServerLink 和 ServiceClientLink 对 象 层 连接 的 过 程 与 发 布 /订阅 类 似 。 
只 有 当 客 户 端 与 服务 端的 服务 类 型 和 md5 校 验 值 匹 配 时 ， 连 接 才 会 成 功 建立 。 
(2) 具体 实现 过 程 


客户 端 在 ServiceClient: : call () 函数 中 创建 一 个 ServiceServerLink 对 象 ， 并 建立 TCPROS 连 接 。 具 体 如 下 ， 首 先 通 过 调用 ServiceManager: : lookupService () 函数 获取 服务 器 端的 主机 号 和 端 
口号 ， 然 后 创建 TransportTCP 对 象 、Connection 对 象 和 ServiceServer-Link 对 象 ， 并 通过 ServiceServerLink: : initialize () 函数 发 送 一 个 header 给 服务 器 ， 设 置 Connection 对 象 收 到 header 的 回调 国 
数 onHeaderReceived () ， 该 回调 函数 完成 向 服务 器 端 发 送 服务 请 求 的 功能 。 


服务 器 端 也 是 在 全 局 TCP 端 口上 进行 监听 ， 当 TransportTCP 层 的 listen () 函数 返回 时 创建 一 个 Connection 对 象 ， 并 设置 该 对 象 在 收 到 header 后 调用 回调 函数 onConnection-HeaderReceived () , 
其 根据 header 创 建 ServiceClientLink 对 象 ， 并 向 客户 端 发 送 一 个 header， 之 后 就 可 以 读 取 来 自 客户 端 的 服务 请 求 了 。 


通过 以 上 过 程 已 经 完成 TCPROS 连 接 建立 ， 之 后 便 可 进行 数据 传输 了 。 
4. 数 据 传送 及 回调 函数 处 理 
(1) 服务 类 型 


与 主题 方式 不 同 ， 服 务 由 一 对 消息 组 成 ， 一 个 用 于 实现 请 求 ， 另 一 个 用 于 返回 经 服务 器 处 理 后 的 响应 。 服 务 类 型 描述 文件 存储 在 srv 文 件 中 。 类 型 描述 文件 被 “---” 分 隔 为 两 部 分 ， 第 一 部 分 为 请 求 的 消 
息 描 述 ， 第 二 部 分 为 响应 的 消息 描述 。 请 求 或 响应 字段 可 以 为 空 ， 甚 至 同时 为 空 


y. 


o 


服务 消息 与 主题 消息 序列 化 的 方法 相同 ， 并 且 在 网 络 中 传输 的 消息 也 为 “length+ content” 格 式 ， 服 务 请 求 消息 与 主题 消息 完全 相同 ， 但 服务 响应 消息 必须 考虑 响应 状态 ， 该 状态 用 来 表征 服务 器 端 函 
数 是 否 调用 成 功 。 所 以 ， 服 务 响 应 消息 的 length 为 5 字 节 ， 其 中 包含 一 个 命名 为 “OK” 的 字 节 ， 用 来 表征 响应 消息 的 类 型 ， 这 个 字 节 位 于 消息 的 首位 。 如 果 该 字 节 的 值 为 {|， 表 示 后 面 的 数据 流 为 正确 的 响应 
消息 ; 如 果 该 字 节 的 值 为 0， 表 示 后 面 的 数据 流 为 一 个 字符 串 类 型 的 错误 提示 。 


图 3-18 和 图 3-19 给 出 了 一 个 简单 的 服务 消息 示例 ， 它 们 分 别 代表 服务 的 请 求 消息 和 响应 消息 ， 处 理 服 务 请 求 的 国 数 实现 将 两 个 数据 a 和 b 相 加 ， 然 后 返回 它们 的 和 sum。 服 务 的 响应 消息 也 可 能 是 
示 ， 如 图 3-20 所 给 的 一 个 示例 。 


错误 提 


00 OI 02 03 uint32 message length 
1000 00 00 16 


04 05 06 07 08 09 0A OB int64 a 


D2 04 00 00 00 00 00 00 1234 


OC OD OE OF 10 11 12 13 int64 b 
2E 16 00 00 00 00 00 00 5678 





A3-18 ”服务 请 求 消息 示例 


OO uints ok 
Ol acknowledge 


0| 02 03 04 uint32 message length 
08 00 00 00 & 


05 06 O7 08 09 OA OB OC int64 sum 
00 1B 00 00 00 00 00 00 691? 





图 3-19 ”服务 响应 消息 示例 


OO uint&8 ok 
QO error 


Ol 02 03 04 05 06 07 08 09 OA OB OC OD OE OF IO 11 string error text 
oD 000000 Unknown error 13 "Unknown error" 





图 3-20 ”服务 响应 错误 消息 示例 
(2) 具体 处 理 流程 


具体 处 理 流 程 如 图 3-21 所 示 。 客 户 端 调用 ServiceClient: : call () 首先 会 建立 TCPROS 连 接 ， 之 后 通过 调用 ServiceServeLink: : call () 请 求 发 送 消息 。ServiceServerLink: : 
processNextCall () 函数 将 客户 端 请 求 消息 发 送 给 服务 器 端 。ServiceServerLink: : onRequestWritten () 函数 阻塞 等 待 来 自 服务 器 端的 响应 消息 。 响 应 消息 逆序 列 化 为 标准 响应 消息 类 型 后 ， 通 过 
ServiceServerLink: callFinished () 函数 唤醒 等 待 线程 。 


服务 器 端 在 向 客户 端 发 送 完 header 后 ， 调 用 ServiceClientLink: : onHeaderWritten () 函数 ， 阻 塞 等 待 读 取 来 自 客户 端 的 请 求 。ServicePublication: : processRequest () 函数 创建 一 个 
serviceCallback 对 象 ， 该 对 象 包含 服务 名 称 、 客 户 端 请 求 、 用 户 编写 的 处 理 请 求 的 回调 函数 等 变量 ， 然 后 将 该 对 象 揪 入 回调 函数 队列 中 。 


轮转 函数 通过 调用 ServiceCallback: : call () 函数 实现 利用 用 户 编写 的 回调 函数 处 理 客 户 端 请 求 消息 。ServiceClientLink: : processResponse () 将 服务 响应 消息 发 送 给 客户 端 。 


ServiceClientLink: : onResponseWritten () 函数 将 根据 不 同 客户 端 执行 不 同 的 操作 ， 如 果 客 户 端 要 求 连接 不 断 开 ， 那 么 将 继续 阻塞 等 待 读 取 来 自 客户 端的 请 求 ; 否则 ， 将 连接 断 开 。 


需要 说 明 的 是 ， 服 务 方式 不 需要 维持 一 个 发 送 服务 调用 队列 ， 服 务 调用 直到 响应 抵达 后 才 会 返回 。 在 默认 情况 下 ， 查 找 和 连接 服务 器 节点 的 过 程 是 在 调用 方法 内 部 发 生 的 。 一 般 情况 下 ， 连 接 仅 在 此 次 
服务 调用 期 间 有 效 ， 在 调用 返回 后 将 被 关闭 。 此 外 ， 通 过 传递 true 值 给 ros: : ServiceClient 构 造 函数 的 第 二 个 参数 ， 可 以 创建 一 个 持续 的 服务 客户 端 ， 建 立 持久 连接 ， 此 后 该 连接 可 被 该 客户 端 对 象 的 所 有 
后 续 调 用 重用 。 


R 和 vie’ cy il 
> H Yini IR Fae iihi 


ServiceServerLink::call() ServiceClientLink::onHeaderWritten() 


ServiceClientLink::onRequestLength() 
ServiceServerLink::processNextCallcall() 


ServiceClientLink::onRequest() 


ServiceServerLink::onRequest Written() ` ServicePublication::process 


创建 ServiceCallback 对 象 ， 插 入 回调 函数 队列 
ServiceServerLink::onResponseOkAndLength() It. ZSEE prey Roly S27 [ul ier Bio be 


ServiceClientLink::call() 
ServiceServerLink::onResponse() 


ServiceClientLink::processResponse() 





ServiceServerLink::callFinishend() ServiceClientLink::onResponseWritten() 


图 3-21 服务 消息 通信 过 程 


3.4 ”基于 参数 服务 器 的 数据 传递 


3.4.1 简介 


参数 服务 器 是 ROS 上 一 种 共享 的 多 变量 参数 字典 ， 节 点 利用 参数 服务 器 获取 和 存储 运行 时 参数 。 参 数 服 务 器 并 不 是 为 了 高 性 能 数据 存储 而 设计 的 ， 其 通常 用 于 存储 一 些 静 态 的 非 二 进 制 数 据 ， 如 配置 参 
数 、 机 器 人 模型 文件 等 。 参 数 服务 器 是 全 局 可 见 的 ， 即 所 有 节点 都 可 以 获取 和 存储 参数 ， 在 必要 时 可 以 删除 、 修 改 参 数 。 基 于 参 服务 器 的 数据 传递 过 程 如 图 3-22 所 示 。 





setParm getParm 





图 3-22 ”基于 参数 服务 器 的 数据 传递 


参数 属于 参数 服务 器 管理 ， 也 就 是 说 ， 即 使 节点 创建 了 参数 ， 但 在 节点 终止 时 参数 仍然 继续 存在 。 虽 然 其 设计 思想 比较 简单 ， 但 其 可 提高 ROS 节 点 的 灵活 性 和 可 配置 性 。 


参数 服务 器 使 用 XMLRPC 实 现 ， 并 且 本 身 运 行 于 ROS 节 点 管理 器 中 ， 这 就 意味 着 它 的 API 可 通过 通用 的 XMLRPC 库 进行 访问 。 


1. 参 数 定义 及 特点 


参数 是 存储 在 节点 管理 器 上 的 全 局 变量 ， 由 索引 和 参数 值 构成 。 节 点 在 运行 时 可 以 在 参数 服务 器 上 存储 和 检索 参数 值 。 参 数 服 务 器 的 存储 结构 是 map<key，value> 的 形式 ， 其 基本 结构 如 图 3-23 所 


人 小。 





图 3-23 ”参数 服务 器 的 存储 结构 


参数 使 用 一 般 的 ROS 命 名 规则 ， 这 就 表明 参数 要 是 用 于 节点 或 主题 ， 就 会 有 与 命名 空间 相 匹 配 的 层次 关系 ， 从 而 避免 发 生 参 数 命名 冲突 。 这 种 层次 体制 允许 节点 在 参数 服务 器 上 获取 单个 参数 或 参数 
树 。 


例如 对 于 下 面 的 参数 : 








/camera/left/name: leftcamera 
/camera/left/exposure: 1 
/camera/right/name: rightcamera 
/camera/right/exposure: 1.1 























其 中 ，“: ”之 前 为 参数 索引 ， 之 后 为 参数 值 。 参 数 /cameraV/left/name 的 值 为 leftcamera， 参 数 /cameray/left/exposure 的 值 为 1， 参 数 /camera/right/name 的 值 为 rightcamera， 参 
数 /camera/right/exposure 的 值 为 1.1。 


1) 获取 单个 参数 的 值 。 例 如 可 以 获取 /camera/left 的 值 ， 那 么 将 会 获得 一 个 字典 : 





name: leftcamera 
exposure: 1 


2) 获取 参数 树 。 例 如 可 获取 /camera 的 值 ， 将 会 得 到 一 个 参数 树 ， 字 典 中 包含 内 容 为 : 





left: { name: leftcamera, exposure: 1 } 
right: { name: rightcamera, exposure: 1.1 } 








参数 服务 器 使 用 XMLRPC 数 据 类 型 为 参数 赋值 ， 为 了 适应 不 同 应 用 类 型 的 数据 传递 ， 该 模式 定义 了 以 下 参数 类 型 。 
- 32-bit integers: 32 位 整数 。 
Booleans: 布尔 值 。 


Strings: F Po 


: Doubles: 双 精 度 浮 点 。 

- iso8601dates: ISO8601 日 期 。 

Lists: 列表 。 

- base64-encoded binary data: 基于 64 位 编码 的 二 进 制 数据 。 

里 然 字 典 具 有 特殊 的 含义 ， 但 是 也 可 以 在 参数 服务 器 上 存储 字典 (如 结构 体 ) 。 与 ROS 命 名 层次 类 似 ， 可 以 在 字典 下 刻 套 字典 来 表示 子 命名 空间 。 


例如 ， 在 参数 gains 上 设置 以 下 三 个 参数 : 











/gains/P = 10.0 
/gains/I = 1.0 
/gains/D = 0.1 


可 以 单独 地 获取 它们 中 的 一 个 ， 如 检测 /gains/P 将 会 返回 10.0。 
也 可 以 获取 整个 字典 ， 当 检索 /gains 时 ， 就 会 返回 一 个 字典 : 


{ "P's 10.0, "I": 1.07 "D" 5 0.1 } 





34 ”基于 参数 服务 器 的 数据 传递 


3.4.1 简介 


参数 服务 器 是 ROS 上 一 种 共享 的 多 变量 参数 字典 ， 节 点 利用 参数 服务 器 获取 和 存储 运行 时 参数 。 参 数 服 务 器 并 不 是 为 了 高 性 能 数据 存储 而 设计 的 ， 其 通常 用 于 存储 一 些 静 态 的 非 二 进 制 数 据 ， 如 配置 参 
数 、 机 器 人 模型 文件 等 。 参 数 服务 器 是 全 局 可 见 的 ， 即 所 有 节点 都 可 以 获取 和 存储 参数 ， 在 必要 时 可 以 删除 、 修 改 参 数 。 基 于 参 服务 器 的 数据 传递 过 程 如 图 3-22 所 示 。 






value 









setParm getParm 





param! | valuel 


| 
| 
| 

ee / 
param’ | value2 | | 
| 
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图 3-22 ”基于 参数 服务 器 的 数据 传递 
参数 属于 参数 服务 器 管理 ， 也 就 是 说 ， 即 使 节点 创建 了 参数 ， 但 在 节点 终止 时 参数 仍然 继续 存在 。 虽 然 其 设计 思想 比较 简单 ， 但 其 可 提高 ROS 节 点 的 灵活 性 和 可 配置 性 。 
参数 服务 器 使 用 XMLRPC 实 现 ， 并 且 本 身 运 行 于 ROS 节 点 管理 器 中 ， 这 就 意味 着 它 的 API 可 通过 通用 的 XMLRPC 库 进行 访问 。 
1. 参 数 定义 及 特点 


参数 是 存储 在 节点 管理 器 上 的 全 局 变量 ， 由 索引 和 参数 值 构成 。 节 点 在 运行 时 可 以 在 参数 服务 器 上 存储 和 检索 参数 值 。 参 数 服务 器 的 存储 结构 是 map<key，value> 的 形式 ， 其 基本 结构 如 图 3-23 所 





图 3-23 ”参数 服务 器 的 存储 结构 


参数 使 用 一 般 的 ROSs 命 名 规则 ， 这 就 表明 参数 要 是 用 于 节点 或 主题 ， 就 会 有 与 命名 空间 相 匹 配 的 层次 关系 ， 从 而 避免 发 生 参 数 命名 冲突 。 这 种 层次 体制 允许 节点 在 参数 服务 器 上 获取 单个 参数 或 参数 
树 。 


例如 对 于 下 面 的 参数 : 








/camera/left/name: leftcamera 
/camera/left/exposure: 1 
/camera/right/name: rightcamera 
/camera/right/exposure: 1.1 























其 中 ，“: ”之 前 为 参数 索引 ， 之 后 为 参数 值 。 参 数 /camera/left/name 的 值 为 leftcamera， 参 数 /camera/left/exposure 的 值 为 1， 参 数 /camera/right/name 的 值 为 rrghtcamera， 参 
数 /camera/right/exposure 的 值 为 1.1。 


1) 获取 单个 参数 的 值 。 例 如 可 以 获取 /camera/left 的 值 ， 那 么 将 会 获得 一 个 字典 : 





name: leftcamera 
exposure: 1 


2) 获取 参数 树 。 例 如 可 获取 /camera 的 值 ， 将 会 得 到 一 个 参数 树 ， 字 典 中 包含 内 容 为 : 











left: { name: leftcamera, exposure: 1 } 
right: { name: rightcamera, exposure: 1.1 } 


2 参数 类 型 


Y 


参数 服务 器 使 用 XMLRPC 数 据 类 型 为 参数 赋值 ， 为 了 适应 不 同 应 用 类 型 的 数据 传递 ， 该 模式 定义 了 以 下 参数 类 型 。 
- 32-bit integers: 32 位 整数 。 

- Booleans: 布尔 值 。 

Strings: 字符 串 。 

Doubles: 双 精 度 浮 点 。 


- iso8601dates: ISO8601 日 期 。 


- Lists: 列表 。 
- base64-encoded binary data: 基于 64 位 编码 的 二 进 制 数据 。 
虽然 字典 具有 特殊 的 含义 ， 但 是 也 可 以 在 参数 服务 器 上 存储 字典 (如 结构 体 ) 。 与 ROS 命 名 层次 类 似 ， 可 以 在 字典 下 许 套 字 上 典 来 表示 子 命名 空间 。 


例如 ， 在 参数 gains 上 设置 以 下 三 个 参数 : 











/gains/P = 10.0 
/gains/I = 1.0 
/gains/D = 0.1 








可 以 单独 地 获取 它们 中 的 一 个 ， 如 检测 /gains/P 将 会 返回 10.0。 
也 可 以 获取 整个 字典 ， 当 检索 /gains 时 ， 就 会 返回 一 个 字典 : 


人 





3.4.2 具体 实现 过 程 


在 roscpp 源 码 中 ， 与 参数 服务 器 相关 的 文件 只 有 param 文 件 ， 该 文件 中 仪 包含 了 ros: : param 命 名 空间 ， 所 以 对 于 参数 服务 器 的 操作 只 与 ros: : param 命 名 空间 相关 。 


参数 服务 器 运行 在 节点 管理 器 (Master) 上 ， 节 点 在 运行 时 可 以 通过 XMLRPC 访 问 所 提供 的 AP1， 实 现在 参数 服务 器 上 根据 索引 设置 或 检索 参数 的 值 。 该 过 程 可 以 抽象 为 图 3-24 所 示 形 式 。 






Master 





Wat XMLRPCLH Ets: 









图 3-24 基于 参数 服务 器 的 数据 传递 过 程 图 


2. 数 据 结构 


在 命名 空间 ros: : param 中 ， 参 数 服务 器 使 用 以 下 参数 : 








typedef std::map<std::string, XmlRpc::XmlRpcValue> M Param; 
M Param g params; E 
boost: :mutex g params mutex; 

S string g | subscribed | | params; 





9g_params 用 来 缓存 参数 列表 ， 当 节点 需要 获取 某 个 参数 的 值 时 ， 可 以 首先 在 g9_params 中 查找 ， 如 果 查 找到 了 就 不 需要 在 参数 服务 器 上 再 查找 ， 这 样 效率 会 比较 高 。 
9g_params_mutex 作 为 互 斥 访问 标志 ， 用 来 保证 某 个 时 刻 只 有 一 个 线程 操作 g_params。 

g_subscribed_params 用 来 存储 所 有 参数 的 索引 ， 以 便 根据 索引 快速 获取 所 需 参 数 的 值 。 

3. 功 能 函数 

roscpp 提 供 的 关于 参数 的 API 支 持 上 述 所 有 数据 类 型 ， 其 中 对 于 字符 串 、 整 数 、 浮 点 型 和 布尔 值 的 使 用 较为 简单 ， 对 其 他 选项 的 支持 则 需 通过 XMLRPC: : xmlrpcvalue 类 。 
参数 服务 器 的 操作 主要 使 用 以 下 几 种 函数 。 

set () : 用 于 在 本 地 缓存 和 在 参数 服务 器 上 存储 参数 。 


get () : 从 参数 服务 器 上 获取 参数 的 值 。 


getCached () : 首先 在 本 地 缓存 中 查找 ， 若 找到 则 直接 在 本 地 缓存 中 获取 参数 的 值 ， 如 果 在 本 地 缓存 中 没有 查找 到 ， 则 在 参数 服务 器 上 订阅 该 参数 ， 之 后 再 在 参数 服务 器 上 获取 参数 对 应 的 值 。 
-has () : 根据 索引 判断 参数 服务 器 是 否 包含 菜 个 参数 。 


-dl () : 在 参数 服务 器 上 删除 指定 参 才 


ev 
o 


‘search () : 根据 给 定 的 索引 ， 在 参数 服务 器 的 树 形 结构 上 查找 对 应 的 值 。 
‘param () : 根据 索引 在 参数 服务 器 上 查找 值 ， 如 果 查 找到 了 ， 则 使 用 查找 到 的 值 。 如 果 没 有 查找 到 ， 则 使 用 默认 值 。 


以 上 函数 可 实现 对 参数 服务 器 的 具体 操作 ， 它 们 均 需 使 用 AMLRPC。 


== 
3.5 本章 小 结 
ROS 是 一 个 分 布 式 的 进程 框架 ,这些 进程 封装 在 易于 分 享 和 发 布 的 功能 包 和 功能 包 集中 ， 每 一 个 进程 就 是 一 个 软件 节点 。ROS 的 运行 框架 是 一 种 使 用 多 模式 ROS 通 信 机 制 实现 节点 之 间 点 对 点 的 松 看 合 
网 络 连 接 的 处 理 架 构 ， 本 章 介绍 了 三 种 主要 的 通信 方式 ， 具 体 如 下 。 
1) 基于 主题 的 异步 数据 流通 信 : 本 章 对 该 通信 方式 作 了 详细 分 析 ， 首 先 概 述 了 通信 过 程 ， 然 后 介绍 了 主题 、XMLRPC、 网 络 消息 格式 等 概念 ， 最 后 分 析 了 其 具体 实现 过 程 。 


2) 基于 服务 的 同步 RPC 通 信 : 本 章 同样 对 该 通信 方式 作 了 详细 分 析 ， 首 先 概述 了 通信 过 程 ， 然 后 介绍 了 服务 、 网 络 消息 格式 等 概念 ， 最 后 分 析 了 有 具体 实现 过 程 。 此 外 ， 对 主题 通信 和 服务 通信 的 区 别 作 
了 简要 分 析 。 


3) 基于 参数 服务 器 的 数据 传递 : 参数 服务 器 运行 于 节点 管理 器 上 ， 主 要 实现 了 一 些 全 局 参数 和 配置 文件 的 存储 ， 实 现 过 程 比 较 简 单 ， 本 章 只 对 使 用 的 数据 结构 和 函数 作 了 简要 介绍 。 


第 4 章 ROS 坐标 变换 体系 及 其 实现 


4.1 机 器 入 运动 学 基本 原理 


4.1.1 空间 坐标 系 的 描述 和 转换 
1. 刚 体 的 位 置 与 姿态 表示 
(1) 位 置 描述 


位 置 是 机 器 人 运动 的 基础 ， 而 其 描述 基于 坐标 系 。 一 旦 建立 了 一 个 坐标 系 ， 我 们 就 可 以 使 用 一 个 3x 1 的 位 置 向 量 来 确定 该 坐标 系 内 任意 一 点 的 位 置 。 在 笛 卡 儿 直 角 坐 标 系 {A} 中 ， 可 以 用 3x1 的 列 向 量 Ap 


— 


P, 
P=| P, | 
Dp; | 


A 





来 描述 空间 任意 一 点 p 的 位 置 。 其 中 ，px、py，p 症 点 p 在 坐标 系 {A} 中 的 三 个 坐标 分 量 。Ap 的 上 标 A 代 表 参 考 坐 标 系 为 TAj。Ap 被 称 为 位 置 矢量 ， 如 图 4-1 所 示 。 


ty 


pP 


Ad 


图 4-1 空间 位 置 矢量 
(2) 方位 描述 


为 了 研究 机 械 臂 的 运动 和 动作 ， 不 仅 要 描述 三 维 空间 某 个 点 的 位 置 ， 而 且 需 要 表示 物体 的 方位 。 物 体 的 方位 可 以 用 某 个 固定 于 此 物体 的 坐标 系 来 描述 。 为 了 规定 空间 某 刚 体 B 的 方位 ， 设 置 一 直角 坐标 系 
{B} 与 此 刚体 固 接 。 


用 坐标 系 {B} 的 三 个 单位 主 向 量 xBp、ypB、ZzB 相 对 于 参考 坐标 系 {A} 的 方向 余弦 组 成 3x 3 的 矩阵 


R R 


A A A 

来 表示 刚体 B 相 对 于 坐标 系 {A} 的 方位 。3 尽 称 为 旋转 矩阵 。 上 式 中 ， 上 标 A 代 表 参 考 系 为 {A}， 下 标 B 表 示 被 表示 的 坐标 系 为 {8}。 4 及 共有 9 个 元 素 ， 但 只 有 3 个 是 独立 的 。 由 于 4 
A 

A, 、44,,、 人 , 均 是 单位 矢量 ， 且 两 两 相互 垂直 ， 故 旋转 矩阵 ® 


的 三 个 列 矢量 


是 正 交 的 ， 且 满足 条 件 


A p” _A R= 


B B 





R= 


R 


A A 
即 的 3 Č epee eke, Ee E aa. 


对 于 单独 绕 x、y 或 z 作 转角 为 6 的 旋转 变换 ， 可 以 分 别 推导 出 三 个 旋转 变换 和 矩阵， 分别 如 下 : 


4 
B 


1 0 0 
R(x,0)=|0 cos@ -sin 
0 sın cos@ 
cosSO 0 sn0 
R(y, 0) =|0 1 0 
-sin 0 cos@ 
cos@ -sin 0 
R(z, 0) =| 0 cosSO 0 
0 0 l 
(3) 位 姿 描 述 
上 面 的 讨论 已 经 描述 了 采用 位 置 矢 量 来 描述 空间 点 的 位 置 、 使 用 旋转 变换 矩阵 来 描述 刚体 的 方位 。 若 要 完全 描述 刚体 B 在 空间 中 的 位 姿 (即位 置 与 姿态 ) ， 则 通常 将 某 一 坐标 系 {B} 与 物体 B 相 固 接 ， 


为 了 方便 建 模 与 分 析 ， 坐 标 系 {B} 的 坐标 原点 一 般 选 取 在 刚体 B 中 有 代表 性 或 有 物理 意义 的 点 上 (如 质心 ) 。 坐 标 系 {} 相 对 于 参考 系 {A} 的 原点 位 置 和 三 坐标 轴 的 方位 ， 分 别 由 位 置 矢量 ^PBo 和 旋转 矩阵 


R ig. 这 样 ， 物 体 B 的 位 姿 就 可 以 由 坐标 系 {B} 描 述 ， 即 : 


2. 空 间 齐 次 坐标 变换 


变换 定义 为 空间 的 一 个 运动 。 当 空间 的 一 个 坐标 系 (一 个 向 量 、 一 个 物体 或 一 个 运动 坐标 系 ) 相对 于 固定 参考 坐标 系 运动 时 ， 这 一 运动 可 以 用 类 似 于 表示 坐标 系 的 方式 来 表示 。 空 间 中 的 任意 点 P 的 数学 


描述 在 不 同 的 坐标 系 中 是 不 同 的 。 为 了 阐明 从 一 个 坐标 系 的 描述 到 另 一 个 坐标 系 的 描述 关系 ， 需 要 讨论 这 种 变换 的 数学 问题 ， 变 换 有 如 下 几 种 类 型 。 


- 单纯 平移 。 
- 绕 某 个 轴 的 单纯 旋转 。 
- 平移 变换 与 旋转 变换 的 复合 。 
首先 我 们 讨论 非 齐 次 坐标 变换 。 并 导出 几 种 类 型 的 变换 在 非 齐 次 坐标 变换 中 的 数学 表示 形式 。 
(1) 非 齐 次 坐标 变换 


若 坐 标 系 {B} 与 {A} 具 有 相同 的 方位 ， 但 {B} 坐 标 系 的 原点 与 {A} 的 原点 不 重合 。 用 矢量 ^PBo 描 述 坐 标 系 {B} 的 原点 相对 于 {A)} 的 位 置 ， 如 图 4-2 所 示 。 称 ^PBO 为 {B} 相 对 于 {A) 的 平移 矢量 。 


如 果 某 一 点 P 在 坐标 系 {B} 中 的 位 置 为 8p， 那 么 它 相对 于 坐标 系 {A} 的 位 置 向 量 可 由 向 量 ^P 相 加 得 出 ， 即 ; 


A. B A 
P= PT Pro 


上 了 式 即 为 坐标 平移 变换 方程 。 





图 4-2 ”坐标 平移 变换 


R 


A 
若 坐 标 系 {Bj 与 {A} 具 有 重合 的 原点 ， 但 两 坐标 系 的 方位 不 同 ， 如 图 4-3 所 示 。 用 旋转 矩阵? 。 来 描述 {B} 相 对 于 {A} 的 方位 。 同 一 点 p 在 两 个 坐标 系 {A} 与 {8} 中 的 位 置 向 量 AP 与 BP 的 变换 关系 可 由 下 式 表示 : 


B 


“p=sp* P 


上 式 即 为 坐标 旋转 变换 方程 。 


R 


4 
对 于 一 般 的 情形 : 坐标 系 {B} 与 {A} 的 原点 不 相同 ， 同 时 两 者 方位 也 不 相同 如 图 4-4 所 示 。 可 以 同时 使 用 平移 矢量 ^PBO 和 旋转 和 矩阵? 来 描述 坐标 系 {B} 到 {A} 的 变换 。 对 于 任意 一 点 p， 其 在 两 个 坐标 系 {A} 


与 {B} 中 的 位 置 矢量 Ap 与 外 有 如 下 变换 关系 : 


A 


A cd 
p= _Re p+ Pro 





O 


图 4-3 ”坐标 旋转 变换 
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图 4-4 平移 与 旋转 复合 变换 


(2) 齐 次 坐标 变换 


上 述 变 换 式 是 非 齐 次 的 ， 其 在 处 理 多 个 变换 复合 时 有 所 不 便 。 不 过 非 齐 次 变换 式 可 以 通过 下 式 转换 为 齐 次 变换 式 ， 且 两 者 是 等 价 的 : 
A A A B 
P B R Peo p 
| 0 l l 
其 中 ， 将 4x1 的 列 向 量 [Ap 11T 和 [Bp 1]T 称 为 点 p 的 齐 次 坐标 ， 仍 然 记 为 AP 或 外 。 上 式 的 矩阵 表达 式 可 记 为 : 
A 4 B 
p= 7,1 p 
A 
式 中 ， 齐 次 坐标 AP 和 BP 是 4x 1 的 列 向 量 ， 两 者 具有 相同 的 维 数 ， 加 入 了 第 四 个 元 素 1。 此 时 章 次 变换 矩阵 ?了 为 4x4 的 方 阵 ， 具 有 以 下 形式 : 


A A 
AT _| B R Po 
B = 
0 l 
Sy 
3 ”综合 地 表示 了 平移 坐标 变换 和 旋转 坐标 变换 。 
特别 地 ， 可 以 得 出 平移 变换 与 单独 绕 某 坐标 轴 旋 转变 换 的 齐 次 坐标 变换 矩 了 咱 ， 并 用 特殊 的 记 法 来 表示 。 


平移 变换 : 


Trans(a,b,c) = 


O © m= © 
So a O © 
m © SO & 


l 
0 
0 
0 


旋转 变换 : 


] 0 0 0 
0 cos@ -sin 0 
Rot(x,0) = 
0 sinô cosO 0 
0 0 0 l 
coSO 0 sind 0 
0 l 0 0 
Rot(y,0) = 
-sin 0 cos 0 
0 0 0 l 
cos@ -sin@ 0 0 
sin cosSO 0 0 
Rot(z, @) = 
0 0 1 0 
0 0 0 1 
如 果 让 一 物体 绕 z 轴 旋转 90*， 接 着 绕 y 轴 旋转 90*， 再 沿 x 轴 方向 平移 5 个 单位 ， 那 么 可 以 用 下 式 来 描述 这 一 变换 : 
0 0 1 4 
T= b R 90) R 90 FU 
= Trans (a, b, c) Rot(y, ot (Z, = 
( ) C ) ( ) sray 
0 0 O d 


(3) 刚体 位 姿 的 矩阵 描述 

一 个 物体 在 空间 的 表示 可 通过 以 下 方式 来 实现 : 在 其 上 固 连 一 个 参考 坐标 系 ， 并 描述 此 坐标 系 的 空间 表示 。 由 于 该 坐标 系 与 物体 的 相对 关系 为 固 连 ， 因 此 可 以 得 到 该 物体 相对 于 参考 坐标 系 的 位 姿 。 综 
上 所 述 ， 若 可 以 得 到 参考 坐标 系 的 空间 表示 ， 则 如 图 4-5 所 示 ， 该 物体 的 相对 固定 坐标 系 的 位 姿 即 可 得 到 。 由 前 述 可 知 ， 可 用 矩阵 表示 空间 坐标 系 ， 则 亦 可 用 矩阵 表示 坐标 原点 及 该 坐标 系 姿态 相对 于 参考 从 
标 系 的 三 个 向 量 。 


让 


O 





r 


X 


图 4-5 刚体 位 姿 描 述 


(4) 复合 变换 
A B 
给 定 坐 标 系 {fAj、{Bj 和 ({C}， 若 已 知人 ) 相 对 于 从 的 齐 次 变换 矩阵 为 8 4，{C} 相 对 于 {Bj 的 齐 次 变换 矩阵 为 C! ， 则 {QO 相 对 于 {A} 的 齐 次 变换 短 阵 可 由 下 式 得 
Ann _ Am B 
c1 = pl cl 
(5) 齐 次 坐标 变换 的 逆 变换 


4 B B 
已 知 {B} 相 对 于 {A) 的 齐 次 变换 矩阵 为 ? e 求 { 久 相对 于 {B} 的 齐 次 变换 矩阵 aT 是 齐 次 变换 的 求 逆 问 题 。 一 种 简单 的 求解 方法 是 直接 对 4x4 的 齐 次 变换 矩阵 aT 进行 求 逆 ， 但 是 运算 量 会 比较 大 。 我 们 可 以 


利用 齐 次 变换 矩阵 的 特点 ， 简 化 其 求 逆 运 算 。 具 体 的 数学 推导 不 再 详 述 ， 其 公式 为 : 

Ant Ant A 
R ~~: © Dox 
() l 


BAE ROS 坐标 变换 体系 及 其 实现 


4.1 机 器 人 运动 学 基本 原理 


4.1.1 ”空间 坐标 系 的 描述 和 转换 
1. 刚 体 的 位 置 与 姿态 表示 
(1) 位 置 描述 


位 置 是 机 器 人 运动 的 基础 ， 而 其 描述 基于 坐标 系 。 一 旦 建立 了 一 个 坐标 系 ， 我 们 就 可 以 使 用 一 个 3x 1 的 位 置 向 量 来 确定 该 坐标 系 内 任意 一 点 的 位 置 。 在 笛 卡 儿 直 角 坐 标 系 {Aj 中 ， 可 以 用 3x1 的 列 向 量 Ap 


— 


Py | 
P=| P, | 
p, | 


A 





HRS ATS APNE. BR, py. py pA Rp EMR A RNS Meine. “pi binAnRES RAIA) Ap, W0B4-1Aim. 


ty 


pP 


Ad 


图 4-1 空间 位 置 矢量 
(2) 方位 描述 


为 了 研究 机 械 臂 的 运动 和 动作 ， 不 仅 要 描述 三 维 空间 某 个 点 的 位 置 ， 而 且 需 要 表示 物体 的 方位 。 物 体 的 方位 可 以 用 某 个 固定 于 此 物体 的 坐标 系 来 描述 。 为 了 规定 空间 某 刚 体 B 的 方位 ， 设 置 一 直角 坐标 系 
{B} 与 此 刚体 固 接 。 


用 坐标 系 {B} 的 三 个 单位 主 向 量 xBp、ypB、ZzB 相 对 于 参考 坐标 系 {A} 的 方向 余弦 组 成 3x 3 的 矩阵 


R R 


A A A 

来 表示 刚体 B 相 对 于 坐标 系 {A} 的 方位 。3 尽 称 为 旋转 矩阵 。 上 式 中 ， 上 标 A 代 表 参 考 系 为 {A}， 下 标 B 表 示 被 表示 的 坐标 系 为 {8}。 4 及 共有 9 个 元 素 ， 但 只 有 3 个 是 独立 的 。 由 于 4 
A 

A, 、44,,、 人 , 均 是 单位 矢量 ， 且 两 两 相互 垂直 ， 故 旋转 矩阵 ® 


的 三 个 列 矢量 


是 正 交 的 ， 且 满足 条 件 


A p” _A R= 


B B 





R= 


R 


A A 
即 的 3 Č epee eke, Ee E aa. 


对 于 单独 绕 x、y 或 z 作 转角 为 6 的 旋转 变换 ， 可 以 分 别 推导 出 三 个 旋转 变换 和 矩阵， 分别 如 下 : 


4 
B 


1 0 0 
R(x,0)=|0 cos@ -sin 
0 sın cos@ 
cosSO 0 sn0 
R(y, 0) =|0 1 0 
-sin 0 cos@ 
cos@ -sin 0 
R(z, 0) =| 0 cosSO 0 
0 0 l 
(3) 位 姿 描 述 
上 面 的 讨论 已 经 描述 了 采用 位 置 矢 量 来 描述 空间 点 的 位 置 、 使 用 旋转 变换 矩阵 来 描述 刚体 的 方位 。 若 要 完全 描述 刚体 B 在 空间 中 的 位 姿 (即位 置 与 姿态 ) ， 则 通常 将 某 一 坐标 系 {B} 与 物体 B 相 固 接 ， 


为 了 方便 建 模 与 分 析 ， 坐 标 系 {B} 的 坐标 原点 一 般 选 取 在 刚体 B 中 有 代表 性 或 有 物理 意义 的 点 上 (如 质心 ) 。 坐 标 系 {} 相 对 于 参考 系 {A} 的 原点 位 置 和 三 坐标 轴 的 方位 ， 分 别 由 位 置 矢量 ^PBo 和 旋转 矩阵 


R ig. 这 样 ， 物 体 B 的 位 姿 就 可 以 由 坐标 系 {B} 描 述 ， 即 : 


2. 空 间 齐 次 坐标 变换 


变换 定义 为 空间 的 一 个 运动 。 当 空间 的 一 个 坐标 系 (一 个 向 量 、 一 个 物体 或 一 个 运动 坐标 系 ) 相对 于 固定 参考 坐标 系 运动 时 ， 这 一 运动 可 以 用 类 似 于 表示 坐标 系 的 方式 来 表示 。 空 间 中 的 任意 点 P 的 数学 


描述 在 不 同 的 坐标 系 中 是 不 同 的 。 为 了 阐明 从 一 个 坐标 系 的 描述 到 另 一 个 坐标 系 的 描述 关系 ， 需 要 讨论 这 种 变换 的 数学 问题 ， 变 换 有 如 下 几 种 类 型 。 


- 单纯 平移 。 
- 绕 某 个 轴 的 单纯 旋转 。 
- 平移 变换 与 旋转 变换 的 复合 。 
首先 我 们 讨论 非 齐 次 坐标 变换 。 并 导出 几 种 类 型 的 变换 在 非 齐 次 坐标 变换 中 的 数学 表示 形式 。 
(1) 非 齐 次 坐标 变换 


若 坐 标 系 {B} 与 {A} 具 有 相同 的 方位 ， 但 {B} 坐 标 系 的 原点 与 {A} 的 原点 不 重合 。 用 矢量 ^PBo 描 述 坐 标 系 {B} 的 原点 相对 于 {A)} 的 位 置 ， 如 图 4-2 所 示 。 称 ^PBO 为 {B} 相 对 于 {A) 的 平移 矢量 。 


如 果 某 一 点 P 在 坐标 系 {B} 中 的 位 置 为 8p， 那 么 它 相对 于 坐标 系 {A} 的 位 置 向 量 可 由 向 量 ^P 相 加 得 出 ， 即 ; 


A. B A 
P= PT Pro 


上 了 式 即 为 坐标 平移 变换 方程 。 





图 4-2 ”坐标 平移 变换 


R 


A 
若 坐 标 系 {Bj 与 {A} 具 有 重合 的 原点 ， 但 两 坐标 系 的 方位 不 同 ， 如 图 4-3 所 示 。 用 旋转 矩阵? 。 来 描述 {B} 相 对 于 {A} 的 方位 。 同 一 点 p 在 两 个 坐标 系 {A} 与 {8} 中 的 位 置 向 量 AP 与 BP 的 变换 关系 可 由 下 式 表示 : 


B 


“p=sp* P 


上 式 即 为 坐标 旋转 变换 方程 。 


R 


4 
对 于 一 般 的 情形 : 坐标 系 {B} 与 {A} 的 原点 不 相同 ， 同 时 两 者 方位 也 不 相同 如 图 4-4 所 示 。 可 以 同时 使 用 平移 矢量 ^PBO 和 旋转 和 矩阵? 来 描述 坐标 系 {B} 到 {A} 的 变换 。 对 于 任意 一 点 p， 其 在 两 个 坐标 系 {A} 


与 {B} 中 的 位 置 矢量 Ap 与 外 有 如 下 变换 关系 : 


A 


A cd 
p= _Re p+ Pro 





O 


图 4-3 ”坐标 旋转 变换 
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图 4-4 平移 与 旋转 复合 变换 


(2) 齐 次 坐标 变换 


上 述 变 换 式 是 非 齐 次 的 ， 其 在 处 理 多 个 变换 复合 时 有 所 不 便 。 不 过 非 齐 次 变换 式 可 以 通过 下 式 转换 为 齐 次 变换 式 ， 且 两 者 是 等 价 的 : 
A A A B 
P B R Peo p 
| 0 l l 
其 中 ， 将 4x1 的 列 向 量 [Ap 11T 和 [Bp 1]T 称 为 点 p 的 齐 次 坐标 ， 仍 然 记 为 AP 或 外 。 上 式 的 矩阵 表达 式 可 记 为 : 
A 4 B 
p= 7,1 p 
A 
式 中 ， 齐 次 坐标 AP 和 BP 是 4x 1 的 列 向 量 ， 两 者 具有 相同 的 维 数 ， 加 入 了 第 四 个 元 素 1。 此 时 章 次 变换 矩阵 ?了 为 4x4 的 方 阵 ， 具 有 以 下 形式 : 


A A 
AT _| B R Po 
B = 
0 l 
Sy 
3 ”综合 地 表示 了 平移 坐标 变换 和 旋转 坐标 变换 。 
特别 地 ， 可 以 得 出 平移 变换 与 单独 绕 某 坐标 轴 旋 转变 换 的 齐 次 坐标 变换 矩 了 咱 ， 并 用 特殊 的 记 法 来 表示 。 


平移 变换 : 


Trans(a,b,c) = 


O © m= © 
So a O © 
m © SO & 


l 
0 
0 
0 


旋转 变换 : 


] 0 0 0 
0 cos@ -sin 0 
Rot(x,0) = 
0 sinô cosO 0 
0 0 0 l 
coSO 0 sind 0 
0 l 0 0 
Rot(y,0) = 
-sin 0 cos 0 
0 0 0 l 
cos@ -sin@ 0 0 
sin cosSO 0 0 
Rot(z, @) = 
0 0 1 0 
0 0 0 1 
如 果 让 一 物体 绕 z 轴 旋转 90*， 接 着 绕 y 轴 旋转 90*， 再 沿 x 轴 方向 平移 5 个 单位 ， 那 么 可 以 用 下 式 来 描述 这 一 变换 : 
0 0 1 4 
T= b R 90) R 90 FU 
= Trans (a, b, c) Rot(y, ot (Z, = 
( ) C ) ( ) sray 
0 0 O d 


(3) 刚体 位 姿 的 矩阵 描述 

一 个 物体 在 空间 的 表示 可 通过 以 下 方式 来 实现 : 在 其 上 固 连 一 个 参考 坐标 系 ， 并 描述 此 坐标 系 的 空间 表示 。 由 于 该 坐标 系 与 物体 的 相对 关系 为 固 连 ， 因 此 可 以 得 到 该 物体 相对 于 参考 坐标 系 的 位 姿 。 综 
上 所 述 ， 若 可 以 得 到 参考 坐标 系 的 空间 表示 ， 则 如 图 4-5 所 示 ， 该 物体 的 相对 固定 坐标 系 的 位 姿 即 可 得 到 。 由 前 述 可 知 ， 可 用 矩阵 表示 空间 坐标 系 ， 则 亦 可 用 矩阵 表示 坐标 原点 及 该 坐标 系 姿态 相对 于 参考 从 
标 系 的 三 个 向 量 。 
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图 4-5 刚体 位 姿 描 述 


(4) 复合 变换 
A B 
给 定 坐 标 系 {fAj、{Bj 和 ({C}， 若 已 知人 ) 相 对 于 从 的 齐 次 变换 矩阵 为 8 4，{C} 相 对 于 {Bj 的 齐 次 变换 矩阵 为 C! ， 则 {QO 相 对 于 {A} 的 齐 次 变换 短 阵 可 由 下 式 得 
Ann _ Am B 
c1 = pl cl 
(5) 齐 次 坐标 变换 的 逆 变换 


4 B B 
已 知 {B} 相 对 于 {A) 的 齐 次 变换 矩阵 为 ? e 求 { 久 相对 于 {B} 的 齐 次 变换 矩阵 aT 是 齐 次 变换 的 求 逆 问 题 。 一 种 简单 的 求解 方法 是 直接 对 4x4 的 齐 次 变换 矩阵 aT 进行 求 逆 ， 但 是 运算 量 会 比较 大 。 我 们 可 以 


利用 齐 次 变换 矩阵 的 特点 ， 简 化 其 求 逆 运 算 。 具 体 的 数学 推导 不 再 详 述 ， 其 公式 为 : 

Ant Ant A 
R ~~: © Dox 
() l 


BAE ROS 坐标 变换 体系 及 其 实现 


4.1 机 器 人 运动 学 基本 原理 


4.1.1 ”空间 坐标 系 的 描述 和 转换 
1. 刚 体 的 位 置 与 姿态 表示 
(1) 位 置 描述 


位 置 是 机 器 人 运动 的 基础 ， 而 其 描述 基于 坐标 系 。 一 旦 建立 了 一 个 坐标 系 ， 我 们 就 可 以 使 用 一 个 3x 1 的 位 置 向 量 来 确定 该 坐标 系 内 任意 一 点 的 位 置 。 在 笛 卡 儿 直 角 坐 标 系 {Aj 中 ， 可 以 用 3x1 的 列 向 量 Ap 


— 


Py | 
P=| P, | 
p, | 


A 
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图 4-1 空间 位 置 矢量 
(2) 方位 描述 


为 了 研究 机 械 臂 的 运动 和 动作 ， 不 仅 要 描述 三 维 空间 某 个 点 的 位 置 ， 而 且 需 要 表示 物体 的 方位 。 物 体 的 方位 可 以 用 某 个 固定 于 此 物体 的 坐标 系 来 描述 。 为 了 规定 空间 某 刚 体 B 的 方位 ， 设 置 一 直角 坐标 系 
{B} 与 此 刚体 固 接 。 


用 坐标 系 {B} 的 三 个 单位 主 向 量 xBp、ypB、ZzB 相 对 于 参考 坐标 系 {A} 的 方向 余弦 组 成 3x 3 的 矩阵 


R R 


A A A 

来 表示 刚体 B 相 对 于 坐标 系 {A} 的 方位 。3 尽 称 为 旋转 矩阵 。 上 式 中 ， 上 标 A 代 表 参 考 系 为 {A}， 下 标 B 表 示 被 表示 的 坐标 系 为 {8}。 4 及 共有 9 个 元 素 ， 但 只 有 3 个 是 独立 的 。 由 于 4 
A 

A, 、44,,、 人 , 均 是 单位 矢量 ， 且 两 两 相互 垂直 ， 故 旋转 矩阵 ® 


的 三 个 列 矢量 


是 正 交 的 ， 且 满足 条 件 


A p” _A R= 


B B 





R= 


R 


A A 
即 的 3 Č epee eke, Ee E aa. 


对 于 单独 绕 x、y 或 z 作 转角 为 6 的 旋转 变换 ， 可 以 分 别 推导 出 三 个 旋转 变换 和 矩阵， 分别 如 下 : 


4 
B 


1 0 0 
R(x,0)=|0 cos@ -sin 
0 sın cos@ 
cosSO 0 sn0 
R(y, 0) =|0 1 0 
-sin 0 cos@ 
cos@ -sin 0 
R(z, 0) =| 0 cosSO 0 
0 0 l 
(3) 位 姿 描 述 
上 面 的 讨论 已 经 描述 了 采用 位 置 矢 量 来 描述 空间 点 的 位 置 、 使 用 旋转 变换 矩阵 来 描述 刚体 的 方位 。 若 要 完全 描述 刚体 B 在 空间 中 的 位 姿 (即位 置 与 姿态 ) ， 则 通常 将 某 一 坐标 系 {B} 与 物体 B 相 固 接 ， 


为 了 方便 建 模 与 分 析 ， 坐 标 系 {B} 的 坐标 原点 一 般 选 取 在 刚体 B 中 有 代表 性 或 有 物理 意义 的 点 上 (如 质心 ) 。 坐 标 系 {} 相 对 于 参考 系 {A} 的 原点 位 置 和 三 坐标 轴 的 方位 ， 分 别 由 位 置 矢量 ^PBo 和 旋转 矩阵 


R ig. 这 样 ， 物 体 B 的 位 姿 就 可 以 由 坐标 系 {B} 描 述 ， 即 : 


2. 空 间 齐 次 坐标 变换 


变换 定义 为 空间 的 一 个 运动 。 当 空间 的 一 个 坐标 系 (一 个 向 量 、 一 个 物体 或 一 个 运动 坐标 系 ) 相对 于 固定 参考 坐标 系 运动 时 ， 这 一 运动 可 以 用 类 似 于 表示 坐标 系 的 方式 来 表示 。 空 间 中 的 任意 点 P 的 数学 


描述 在 不 同 的 坐标 系 中 是 不 同 的 。 为 了 阐明 从 一 个 坐标 系 的 描述 到 另 一 个 坐标 系 的 描述 关系 ， 需 要 讨论 这 种 变换 的 数学 问题 ， 变 换 有 如 下 几 种 类 型 。 


- 单纯 平移 。 
- 绕 某 个 轴 的 单纯 旋转 。 
- 平移 变换 与 旋转 变换 的 复合 。 
首先 我 们 讨论 非 齐 次 坐标 变换 。 并 导出 几 种 类 型 的 变换 在 非 齐 次 坐标 变换 中 的 数学 表示 形式 。 
(1) 非 齐 次 坐标 变换 


若 坐 标 系 {B} 与 {A} 具 有 相同 的 方位 ， 但 {B} 坐 标 系 的 原点 与 {A} 的 原点 不 重合 。 用 矢量 ^PBo 描 述 坐 标 系 {B} 的 原点 相对 于 {A)} 的 位 置 ， 如 图 4-2 所 示 。 称 ^PBO 为 {B} 相 对 于 {A) 的 平移 矢量 。 


如 果 某 一 点 P 在 坐标 系 {B} 中 的 位 置 为 8p， 那 么 它 相对 于 坐标 系 {A} 的 位 置 向 量 可 由 向 量 ^P 相 加 得 出 ， 即 ; 


A. B A 
P= PT Pro 


上 了 式 即 为 坐标 平移 变换 方程 。 





图 4-2 ”坐标 平移 变换 


R 


A 
若 坐 标 系 {Bj 与 {A} 具 有 重合 的 原点 ， 但 两 坐标 系 的 方位 不 同 ， 如 图 4-3 所 示 。 用 旋转 矩阵? 。 来 描述 {B} 相 对 于 {A} 的 方位 。 同 一 点 p 在 两 个 坐标 系 {A} 与 {8} 中 的 位 置 向 量 AP 与 BP 的 变换 关系 可 由 下 式 表示 : 


B 


“p=sp* P 


上 式 即 为 坐标 旋转 变换 方程 。 


R 


4 
对 于 一 般 的 情形 : 坐标 系 {B} 与 {A} 的 原点 不 相同 ， 同 时 两 者 方位 也 不 相同 如 图 4-4 所 示 。 可 以 同时 使 用 平移 矢量 ^PBO 和 旋转 和 矩阵? 来 描述 坐标 系 {B} 到 {A} 的 变换 。 对 于 任意 一 点 p， 其 在 两 个 坐标 系 {A} 


与 {B} 中 的 位 置 矢量 Ap 与 外 有 如 下 变换 关系 : 


A 


A cd 
p= _Re p+ Pro 





O 


图 4-3 ”坐标 旋转 变换 





X 4 


图 4-4 平移 与 旋转 复合 变换 


(2) 齐 次 坐标 变换 


上 述 变 换 式 是 非 齐 次 的 ， 其 在 处 理 多 个 变换 复合 时 有 所 不 便 。 不 过 非 齐 次 变换 式 可 以 通过 下 式 转换 为 齐 次 变换 式 ， 且 两 者 是 等 价 的 : 
A A A B 
P B R Peo p 
| 0 l l 
其 中 ， 将 4x1 的 列 向 量 [Ap 11T 和 [Bp 1]T 称 为 点 p 的 齐 次 坐标 ， 仍 然 记 为 AP 或 外 。 上 式 的 矩阵 表达 式 可 记 为 : 
A 4 B 
p= 7,1 p 
A 
式 中 ， 齐 次 坐标 AP 和 BP 是 4x 1 的 列 向 量 ， 两 者 具有 相同 的 维 数 ， 加 入 了 第 四 个 元 素 1。 此 时 章 次 变换 矩阵 ?了 为 4x4 的 方 阵 ， 具 有 以 下 形式 : 


A A 
AT _| B R Po 
B = 
0 l 
Sy 
3 ”综合 地 表示 了 平移 坐标 变换 和 旋转 坐标 变换 。 
特别 地 ， 可 以 得 出 平移 变换 与 单独 绕 某 坐标 轴 旋 转变 换 的 齐 次 坐标 变换 矩 了 咱 ， 并 用 特殊 的 记 法 来 表示 。 


平移 变换 : 


Trans(a,b,c) = 


O © m= © 
So a O © 
m © SO & 


l 
0 
0 
0 


旋转 变换 : 


] 0 0 0 
0 cos@ -sin 0 
Rot(x,0) = 
0 sinô cosO 0 
0 0 0 l 
coSO 0 sind 0 
0 l 0 0 
Rot(y,0) = 
-sin 0 cos 0 
0 0 0 l 
cos@ -sin@ 0 0 
sin cosSO 0 0 
Rot(z, @) = 
0 0 1 0 
0 0 0 1 
如 果 让 一 物体 绕 z 轴 旋转 90*， 接 着 绕 y 轴 旋转 90*， 再 沿 x 轴 方向 平移 5 个 单位 ， 那 么 可 以 用 下 式 来 描述 这 一 变换 : 
0 0 1 4 
T= b R 90) R 90 FU 
= Trans (a, b, c) Rot(y, ot (Z, = 
( ) C ) ( ) sray 
0 0 O d 


(3) 刚体 位 姿 的 矩阵 描述 

一 个 物体 在 空间 的 表示 可 通过 以 下 方式 来 实现 : 在 其 上 固 连 一 个 参考 坐标 系 ， 并 描述 此 坐标 系 的 空间 表示 。 由 于 该 坐标 系 与 物体 的 相对 关系 为 固 连 ， 因 此 可 以 得 到 该 物体 相对 于 参考 坐标 系 的 位 姿 。 综 
上 所 述 ， 若 可 以 得 到 参考 坐标 系 的 空间 表示 ， 则 如 图 4-5 所 示 ， 该 物体 的 相对 固定 坐标 系 的 位 姿 即 可 得 到 。 由 前 述 可 知 ， 可 用 矩阵 表示 空间 坐标 系 ， 则 亦 可 用 矩阵 表示 坐标 原点 及 该 坐标 系 姿态 相对 于 参考 从 
标 系 的 三 个 向 量 。 
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图 4-5 刚体 位 姿 描述 
(4) 复合 变换 


T 


A B 
给 定 坐标 系 {A、{B) 和 {C}， 若 已 知人 )}) 相 对 于 { 和 的 齐 次 变换 矩阵 为 8 r, (CHAI FBI FPR REAC , MCHA HAAI PARRE E EB RS : 


T=] eT 


(5) 齐 次 坐标 变换 的 逆 变 换 


4 B B 
已 知 {B} 相 对 于 {A) 的 齐 次 变换 矩阵 为 ? e 求人 从 相 对 于 {B]} 的 齐 次 变换 矩阵 aT 是 齐 次 变换 的 求 逆 问 题 。 一 种 简单 的 求解 方法 是 直接 对 4x4 的 齐 次 变换 矩阵 aT 进行 求 逆 ， 但 是 运算 量 会 比较 大 。 我 们 可 以 
利用 齐 次 变换 矩阵 的 特点 ， 简 化 其 求 逆 运 算 。 具 体 的 数学 推导 不 再 详 述 ， 其 公式 为 : 


412 ”机 器 人 的 正 运动 学 


1. 机 器 人 运动 学 概念 


对 于 一 个 机 器 人 ， 若 已 知 它 的 所 有 连 杆 长 度 和 关节 和 角度 ， 即 构 型 已 知 ， 则 对 其 手 壁 的 位 姿 分 析 被 称 为 正 运 动 学 分 析 。 根 据 正 运动 学 分 析 ， 对 于 既定 机 器 人 ， 若 已 知 任意 瞬间 其 所 有 的 关节 变量 ， 则 可 以 
计算 出 该 瞬间 机 器 人 的 位 姿 。 反 之 ， 若 必须 得 到 机 器 人 的 所 有 连 杆 长 度 和 关节 和 角度， 才能 将 机 器 人 的 手 部 定位 在 期 望 的 位 次 上 ， 则 该 过 程 称 为 逆 运 动 学 分 析 。 也 就 是 说 ， 该 过 程 不 是 把 机 器 人 的 相关 已 知 变 
量 代入 正 向 运动 学 方程 中 ， 而 是 计算 得 到 这 些 方程 的 解 ， 从 而 求 得 所 需 的 关节 变量 ， 使 机 器 人 处 于 期 望 的 位 姿 。 


本 书 只 讨论 机 器 人 的 正 运动 学 分 析 。 


机 器 人 的 机 械 手 和 相关 部 分 可 看 作 由 若干 关节 连接 起 来 的 连 杆 组 成 的 。 为 此 ， 若 根据 机 械 手 的 连 杆 建立 坐标 系 ， 即 为 每 个 连 杆 均 建立 一 个 坐标 系 ， 则 这 些 不 同 连 杆 坐 标 系 间 的 相对 位 置 关 系 可 以 采用 齐 
次 坐标 变换 矩阵 表示 。 其 中 单个 矩阵 摘 述 的 是 相 邻 连 杆 之 间 的 相对 位 置 关系 ， 具 体 来 说 ， 就 是 相对 平移 和 旋转 的 齐 次 变换 。 本 书 中 将 这 类 和 矩阵 表示 为 矩阵 A， 定 义 顺序 为 从 基 坐 标 系 到 末端 控制 器 之 间 的 坐 
标 系 间 齐 次 变换 矩阵 分 别 为 A1，A2.…。 则 第 二 个 连 杆 相对 于 基 坐 标 系 的 位 置 和 姿态 可 由 A1 与 Az 的 乘积 给 出 ， 即 : 


同 理 ， 若 A3 表 示 第 三 个 连 杆 相对 于 第 二 个 连 杆 的 位 置 和 姿态 ， 则 有 : 


对 于 6 连 杆 的 机 械 结 构 ， 有 下 列 T 甜 阵 : 


E 


6 =Á, A, A, A, A, A, 
6 连 杆 的 机 械 结 具 有 6 个 自由 度 ， 即 每 个 自由 度 对 应 一 个 连 杆 ， 该 结构 能 在 其 可 运动 范围 内 进行 任意 定位 与 定向 。 
2 机 器 人 正 运动 学 方程 的 D-H 表 示 


Denavit 和 Hartenberg 于 1955 年 在 ASME Journal of Applied Mechanics 发 表 了 一 篇 论文 ， 后 人 利用 这 篇 论文 给 出 的 方法 对 机 器 人 进行 表示 和 | 建 模 ， 并 导出 了 其 运动 方程 ， 该 运动 方程 现 已 成 为 对 机 器 
人 进行 表示 及 运动 建 模 的 经 典 方法 ， 称 之 为 Denavit-Hartenberg (简化 为 D-H) 模型 方法 。 该 方法 是 针对 任何 构 型 、 结 构 顺 序 和 复杂 程度 机 器 人 的 一 种 连 杆 和 关节 建 模 方法 ， 亦 可 用 于 解决 如 直角 坐标 、 
柱 坐 标 、 球 坐标 、 欧 拉 角 坐标 及 RPY 坐 标 等 任意 坐标 系 中 的 变换 问题 。 同 时 ， 该 方法 可 表示 任意 连 杆 和 关节 组 合 ， 如 全 旋转 的 链 式 机 器 人 、SCARA 机 器 人 。 昌 然 对 机 器 人 采用 前 文 所 述 的 方法 建 模 更 加 直接 
和 省 时 ， 但 是 D-H 模 型 方法 具有 一 些 附加 的 好 处 ， 目 前 其 已 经 在 许多 问题 中 得 到 了 应 用 ， 如 计算 Jacobian 和 矩阵 和 受 力 分 析 等 。 


若 将 机 器 人 简化 为 一 组 连 杆 和 关节 ， 则 这 些 关 节 是 滑动 (线性 ) 的 或 是 旋转 (转动 ) 的 ， 它 们 可 以 按 任意 顺序 放置 于 任意 平面 中 。 连 杆 可 以 被 赋予 任意 长 度 (包括 零 ) ， 它 可 以 被 弯曲 或 扭曲 ， 也 可 以 
分 布 于 任意 平面 中 。 所 以 任何 一 组 关节 和 连 杆 都 可 以 构成 一 个 我 们 想 要 建 模 和 表示 的 机 器 人 。 因 此 待 分 析 的 机 器 人 可 以 由 任意 一 组 连 杆 和 关节 表示 和 建 模 。 


上 文 所 述 方法 的 前 提 是 每 个 关节 需要 对 应 一 个 参考 坐标 系 ， 并 能 得 到 相 邻 单个 或 若干 个 坐标 系 变换 表示 。 由 此 将 这 些 坐标 系 变换 进行 晋 加 ， 就 能 得 到 从 机 器 人 基 座 到 末端 控制 器 之 间 的 坐标 变换 总 矩 
阵 。 本 节 将 详 述 使 用 D-H 模 型 方法 定义 参考 坐标 系 和 实现 坐标 系 变换 的 具体 步骤 。 


可 将 机 器 人 简化 为 一 堆 以 任意 形式 组 成 的 连 杆 和 关节 。 图 4-6 示 意 了 两 个 依次 连接 的 连 杆 和 对 应 的 三 个 关节 ， 这 些 关节 可 以 旋转 或 滑动 ， 因 此 该 示意 图 可 用 来 模拟 实际 机 器 人 的 任意 关节 和 连接 方式 。 
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图 4-6 ”D-H 方法 中 的 关节 模型 


尽管 在 实际 情况 下 ， 机 器 人 的 关节 通常 只 有 一 个 自由 度 ， 但 图 4-6 中 的 关节 可 以 表示 一 个 或 两 个 自由 度 。 


图 4-6 中 所 述 的 三 个 关节 ， 每 个 关节 都 是 可 以 转动 或 平移 的 。 将 第 1 个 关节 假定 为 关节 n， 第 2 个 关节 为 关节 n+1， 第 3 个 关节 为 关节 n+2。 每 个 关节 都 可 以 旋转 或 滑动 。 对 连 杆 的 定义 为 : 介 于 关节 n 与 
n+1 之 间 的 为 连 杆 n， 关 节 n+1 与 n+2 之 间 的 为 连 杆 n+1。 


使 用 D-H 模 型 方法 来 建 模 机 器 人 ， 首 先 需要 为 每 个 关节 建立 一 个 本 地 参考 坐标 系 。 其 中 因为 我 们 常用 的 直角 坐标 系 只 需要 指定 任意 两 个 轴 ， 第 三 个 轴 会 自动 定义 ， 故 这 里 的 D-H 模 型 方法 只 需 指定 x 和 zZ 
轴 。 下 面 给 出 单个 关节 本 地 参考 坐标 系 建 模 的 详细 流程 ， 有 具体 如 下 。 


1) 所 有 关节 均 用 z 轴 表示 。 如 果 关 节 是 平移 的 ， 那 么 关节 运动 的 方向 即 为 z 轴 。 如 果 关 节 是 转动 的 ， 根 据 右 手 准则 即 可 得 到 z 轴 的 方向 。 在 上 述 两 种 情形 下 ， 关 节 n 处 的 z 轴 的 下 标 均 为 n-1。 对 于 平移 的 
关节 ， 连 杆 长 度 dq 即 z 轴 的 长 度 ， 即 为 关节 变量 。 对 于 转动 关节 ， 连 杆 转动 角度 〈6 角 ) ， 即 绕 z 轴 的 转角 ， 即 为 天 节 变 量 。 以 此 类 推 ， 我 们 可 以 得 到 所 有 关节 的 z 轴 和 关节 变量 。 


2) 一 般 情 况 下 关节 不 会 相交 或 平行 ， 如 图 4-6 所 示 。 尽 管 z 轴 一 般 不 是 垂直 的 ， 但 是 在 任意 两 个 倾斜 的 z 轴 之 间 通 常 有 一 条 公 垂 线 ， 其 距离 最 短 。x 轴 通常 定义 为 公 垂 线 的 方向 。 因 此 ， 若 an 为 zn+1 与 
zn 两 轴 的 公 垂 线 ， 那 么 即 沿 的 方向 。 同 理 ， 若 an+1 为 zn 与 zn+1 两 轴 的 公 垂 线 ， 那 么 xn+1 即 与 an+1 的 方向 一 致 。 同 理 ， 相 邻 关节 的 公 垂 线 也 不 一 定 共 线 或 相交 ， 这 会 导致 两 个 相 邻 坐标 系 的 原点 位 置 不 一 定 重 
合 。 由 此 再 考虑 以 下 列举 的 特殊 情况 ， 即 可 为 所 有 的 关节 定义 本 地 参考 坐标 系 。 


3) 若 相 邻 关节 的 z 轴 相互 相交 ， 那 么 它们 之 间 就 没有 公 重 线 (或 者 说 公 垂 线 距离 为 零 ) 。 这 时 可 将 垂直 于 两 条 轴线 构成 的 平面 的 直线 定义 为 x 轴 。 也 就 是 说 ， 其 公 垂 线 是 垂直 于 包含 了 两 条 z 轴 的 平面 的 
直线 ， 它 也 相当 于 选取 两 条 z 轴 的 又 积 方向 作为 x 轴 。 这 也 会 使 模型 得 以 简化 。 


4) 若 相 邻 关节 的 z 轴 相互 平行 ， 则 两 者 之 间 存在 的 公 重 线 数目 为 无 穷 。 此 时 为 了 简化 模型 ， 公 委 线 选 为 与 前 一 关节 的 公 王 线 共 线 的 任 一 条 即 可 . 
图 4-6 中 ， 绕 z 轴 的 旋转 角 即 为 9 角 ，z 轴 上 两 条 相 邻 公 垂 线 相距 为 d。 通 常 ， 只 有 9 和 d 是 关节 变量 
接 下 来 还 需要 解决 参考 坐标 系 间 变换 的 问题 。 假 定 目前 本 地 参考 坐标 系 为 xn-zn， 则 通过 以 下 若干 标准 运动 即 可 到 达 相 邻 本 地 参考 坐标 系 xn +1-zn+1。 
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xor 


绕 zn 轴 旋转 6n+1 (如 图 4-6 与 图 4-7a 所 示 ) ， 它 使 得 xn 和 xn+1 互 相 平行 ， 因 为 an 和 an+6 均 垂直 于 zn 轴 的 ， 因 此 绕 zn 轴 旋转 6n+1 使 其 相互 平行 (FABRE). 


2 


xr 


沿 zn 轴 平移 dn+1 距 离 ， 使 得 xn 和 xn+1 共 线 (如 图 4-7b 所 示 ) . AAxx BA HTF REET Zy, REZIDAR REER, 
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xr 


沿 xn 轴 平 移 an+1 的 距离 ， 使 得 和 的 原点 一 致 (如 图 4- 7c 和 d 所 示 ) 。 此 时 两 个 参考 坐标 系 的 原点 重合 。 


A) 将 zn 轴 绕 xn+1 轴 旋转 an+1， 使 得 zn 轴 与 zn+1 轴 对 准 (如 图 4-7e 所 示 ) 。 这 时 坐标 系 n 和 n+1 保 持 一 致 (如 图 4-7f 所 示 ) 。 至 此 ， 相 邻 坐 标 系 间 的 坐标 变换 已 完成 。 
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图 4-7 D-H 变换 步骤 


同 理 ，n+1 和 n+2 坐 标 系 间 也 可 按照 上 文 所 述 的 4 个 步骤 将 当前 坐标 变换 到 相 邻 坐标 系 。 对 于 更 复杂 的 情况 ， 重 复 以 上 4 个 步骤 ， 一 系列 相 邻 坐标 系 之 间 的 依次 变换 即 可 实现 。 对 于 多 连 杆 装置 的 机 械 辟 
机 构 ， 我 们 将 第 一 个 参考 坐标 系 转换 到 机 器 人 的 基 座 ， 之 后 依次 转换 到 第 一 个 关节 、 第 二 个 关节 ， 等 等 ， 最 终 到 达 末 端 执行 器 。 上 述 过 程 虽 然 步骤 较 多 ， 但 过 程 大 多 重复 ， 较 易 实 现 。 


鉴于 所 有 的 变换 都 是 相对 于 当前 坐标 系 的， 即 所 有 数据 均 为 相对 于 当前 坐标 系 来 测量 与 执行 得 到 的 ， 因 此 矩 阵 之 间 的 变换 均 可 用 右 乘 实现 。 例 如 ， 有 4 个 矩 阵 来 表示 4 项 运动 ， 则 通过 右 乘 这 些 和 矩阵 即 可 
得 到 总 体 变换 矩阵 A， 此 时 和 矩阵 A 代 表 了 4 项 连续 的 变换 ， 即 4 项 连续 的 运动 。 结 果 如 下 : 
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式 中 C 和 S 分 别 为 cos 和 sin 的 缩写 。 


从 机 器 人 的 基 座 开始 ， 依 次 由 最 接近 基 座 的 关节 一 变换 到 关节 二 ， 等 等 ， 直 至 最 终 变 换 到 机 器 人 的 手 部， 到 达 末 端 执行 器 。 如 果 定 义 每 个 变换 为 Ai， 则 这 些 若干 变换 最 终 都 能 用 矩阵 的 形式 表达 出 来 。 
机 器 人 基 座 和 手 部 之 间 的 总 变换 可 用 和 矩 阵 表 达 为 : 


7, sB rT = i A 


n 


ERNEA. A2-MlaASB6T ERE, WARMER MBIA, 


本 书 制作 了 一 份 与 机 械 臂 关节 和 连 杆 参 数 相 关 的 表格 ， 用 于 简化 矩阵 A 的 计算 。 该 表 中 的 各 项 参数 均 可 以 从 机 器 人 原理 示意 图 中 得 出 ， 将 这 些 参数 带 入 和 矩阵 A， 则 可 得 到 表 4-1。 
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4.2 ROS tf 


机 器 人 想 要 完成 预定 的 任务 ， 必 须 清楚 自身 及 周围 环境 的 位 置 天 系 。 例 如 ， 当 我 们 需要 让 一 个 带 有 机 械 爪 的 机 器 人 抓 取 一 个 球 时 ， 为 了 完成 这 一 简单 任务 ， 就 必须 得 到 球 和 夹 持 器 之 间 的 位 置 关系 。 为 
了 获取 这 一 位 置 关 系 可 能 需要 布置 一 些 传感器 。 传 感 器 得 到 了 球 相对 于 自身 的 位 置 ， 然 后 需要 进行 坐标 转换 ， 才 能 得 到 球 相对 于 机 器 人 基 座 的 位 置 、 相 对 于 机 器 人 手腕 的 位 置 ， 以 及 相对 于 最 终 夹 持 器 的 位 
置 。 机 器 人 模型 的 坐标 结构 简 图 如 图 4-8 所 示 。 


如 图 4-8 所 示 ， 图 中 的 每 个 三 脚 架 均 表示 一 个 坐标 系 ， 它 们 之 间 存 在 着 一 定 的 转换 关系 。 


在 ROS 的 早期 发 展 阶段 ， 坐 标 系 相关 的 计算 被 认为 是 一 个 几乎 所 有 机 器 人 开发 者 都 会 遇 到 的 难点 ， 由 于 坐标 系 变换 是 一 个 比较 复杂 的 问题 ， 同 时 ， 在 机 器 人 系统 中 ， 坐 标 系 之 间 的 变换 数据 往往 是 分 布 
式 的 ， 这 也 使 其 成 为 开发 者 面临 的 一 个 技术 挑战 。 





图 4-8 机 器 人 模型 的 坐标 结构 简 图 


ROS tf (transform) 库 的 目的 是 为 了 提供 一 种 标准 的 手段 跟踪 坐标 系 ， 以 及 整个 系统 内 的 坐标 变换 数据 。tf 使 得 其 他 构件 可 以 方便 、 放 心地 使 用 坐标 系 中 的 数据 ， 而 不 需要 了 解 整个 系统 内 的 坐标 系 细 
节 。 机 器 人 系统 变 得 越 来 越 复 杂 ， 对 于 任何 一 个 构件 ， 都 能 够 专注 于 自身 的 任务 ， 因 此 ， 提 供 相 关 的 坐标 系 及 其 数据 是 很 重要 的 。 


4.3 tf 原理 分 析 


4.3.1 相关 数据 结构 


ROS tf 为 了 实现 坐标 变换 ， 设 计 了 相关 数据 结构 ， 描 述 坐 标 系 及 其 转换 关系 。 为 了 深入 分 析 这 些 数据 结构 的 组 成 及 其 作用 ， 我 们 将 其 分 为 数学 相关 和 转换 相关 两 类 数据 结构 。 
1. 数 学 相关 的 数据 结构 及 其 基本 范 数 
(1) 空间 点 和 空间 向 量 描述 一 一 Vector3 


在 tf 功能 包 的 头 文件 Vector3.h 中 定义 了 Vector3 类 ， 用 来 表示 三 维 空间 的 一 个 点 或 一 个 向 量 。Vertor3 定 义 的 一 个 主要 成 员 变 量 为 : 





tfScalarm floats[4];  // tfScalar 相当 于 double 








这 里 用 x、y、z、w 分 别 来 表示 m floats[0]、m floats[1]、m floats[2]、m floats[3]。 
其 中 x、y、z 对 应 三 维 空间 的 三 个 坐标 参数 ， 而 w 则 用 于 控制 数据 是 否 对 齐 ， 正 常情 况 下 为 0.0。 


成 员 变 量 m_floats 表 示 一 进行 个 向 量 或 一 个 点 。 对 于 一 个 点 来 说 ， 独 立 计算 比较 少 ， 后 文 将 结合 齐 次 变换 再 进行 一 定 阅 明 ， 这 里 主要 介绍 向 量 处 理 及 其 计算 。 向 量 主 要 用 于 表示 坐标 系 转化 中 坐标 原点 
之 间 的 平移 天 系 。 


Vector3 中 定义 了 一 组 与 向 量 相关 的 操作 遂 数 ， 具 体 的 函数 如 表 4-2 所 示 。 


表 4-2 Vector3 类 中 定义 的 主要 函数 


编写 PREZ FR 函数 功能 


] tfScalar ttDot(const Vector3& vl, const Vector3& v2) 计算 两 个 回 量 的 点 积 (内 积 ) 


2 tfScalar lengthQ 计算 向 量 的 长 度 ( 模 ) 
3 Vector3& normalize() 求 与 已 知 向 量 同 向 的 单位 向 量 





4 tfScalartfAngle(const Vector3& vl, const Vector3& v2) 计算 两 个 回 量 的 夹 角 
$ tfScalartfDistance(const Vector3& vl, const Vector3v2) 计算 两 个 回 量 之 间 的 距离 
6 Vector3 tfCross(const Vector3& vl, const Vector3& v2) 计算 两 个 回 量 的 叉 积 (外 积 ) 
void setInterpolate3(const Vector3& v0, const Vector3& vl,| 利用 已 知 的 两 个 回 量 线性 插值 得 到 介 
tfScalar rt) 于 两 者 之 轩 的 一 个 近似 回 量 
8 Vector3 rotate( const Vector3& wAxis, const tfScalar angle ) 计算 指定 www- SEER AE Se l BEDER A 
FÈ angle 之 后 得 到 的 回 量 





除了 表 4-2 中 所 列 的 函数 之 外 ， 革 还 定义 了 一 些 向 量 基 本 运算 的 函数 ， 如 向 量 之 间 的 加 关系 、 减 天 系 、 乘 关系 (不 同 于 内 积 和 叉 积 ) 、 除 天 系 等 。 利 用 这 些 函 数 及 表格 中 所 述 的 函数 ，tf 可 以 很 好 地 处 理 
不 同 向 量 之 间 的 关系 。 已 知 两 个 向 量 的 坐标 ， 可 以 利用 表格 中 的 相关 函数 计算 得 到 它们 之 间 的 夹 角 和 距离 ， 以 便于 后 续 操作 的 继续 进行 ， 进 行 旋转 或 平移 。 


Vector3 中 定义 的 成 员 变 量 m_floats 不 仅 可 以 独立 表示 一 个 向 量 或 是 一 个 点 ， 同 时 在 定义 后 述 的 Matrix3x3 (表示 一 个 3x3 和 矩 阵 ) 时 ， 它 也 可 以 作为 Matrix3x3 的 成 员 变 量 中 的 一 行 。 在 处 理 Matrix3x3 的 
相关 计算 时 ， 把 其 中 的 一 行当 成 一 个 整体 并 且 利 用 Vector3 中 定义 的 向 量 处 理 函 数 ， 也 能 使 处 理 数 据 变 得 更 加 有 效率 ， 这 也 说 明 Vector3 类 一 个 好 的 设计 对 提升 整个 系统 的 计算 效率 是 很 有 必要 的 。 





在 tf 功能 包 的 头 文 件 Quaternion.h 中 定义 了 Quaternion 类 ， 它 用 来 表示 在 三 维 空间 当中 的 一 次 纯 旋转 。Quaternion 类 继承 于 它 的 父 类 QuadWord。 在 QuadWord 类 当中 主要 定义 的 成 员 变 量 如 下 : 








tfScalar m floats[4];  // tfScalar 相当 于 double 














本 节 用 x、y、z、w 分 别 来 表示 m _floats[0]、m _floats[1]、m _floats[2]、m _floats[3]。 一 般 来 说 ， 有 旋转 矩阵 、 欧 拉 旋 转 及 四 元 数 三 种 方式 表示 旋转 。 


旋转 矩阵 是 使 用 一 个 特定 的 矩阵 来 表示 绕 任 意 轴 旋转 。 欧 拉 旋 转 则 是 通过 定义 坐标 轴 顺 序 (例如 先 x、 再 y、 最 后 z) 和 绕 每 个 轴 旋 转 的 角度 来 表示 一 次 旋转 ， 实 际 上 是 一 系列 坐标 轴 旋 转 的 组 合 。 


四 元 数 本 质 上 是 一 种 高 阶 复数 ， 是 一 个 四 维 空间 ， 相 当 于 复数 的 二 维 空间 。 复 数 由 实 部 和 虚 部 组 成 ， 即 x=a+bi (i 是 虚数 单位 ，i?=-1) 。 四 元 数 和 复数 的 定义 是 类 似 的， 不 同 的 是 ， 它 的 虚 部 包含 了 三 
个 虚数 单位 ，i、j、k， 即 四 元 数 可 以 表示 为 Q=w+xityj+zk。QuadWord 类 当中 的 成 员 变量 m _floats 的 4 个 变量 分 别 对 应 这 里 的 x，y，z，w。 结 合 有 具体 的 例子 来 说 ， 可 以 这 样 理解 一 个 四 元 数 。 给 定 一 个 单 
位 长 度 的 旋转 轴 (a, b, c) 和 一 个 角度 6， 对 应 的 四 元 数 为 : 


q= (a*sin (6/2) , b*sin (0/2) , c*sin (0/2) , cos (6/2) ) 
三 种 旋转 的 表示 方法 中 ， 每 种 方法 都 有 各 自 的 优 缺 点 。 其 中 四 元 式 表示 方法 有 一 个 重要 的 优点 是 可 以 提供 平滑 的 插值 ， 这 是 ROS 所 必要 且 特 有 的 ， 在 后 续 的 论述 中 会 对 其 的 必要 性 作 相 应 的 解释 。 
QuadWord 类 中 仅仅 定义 了 主要 的 成 员 变 量 ， 而 Quaternion 类 中 则 定义 了 四 元 式 的 相关 操作 函数 ， 具 体 函 数 如 表 4-3 所 示 。 


表 4-3 ”Quatetnion 类 中 定义 的 主要 函数 


编号 函数 功能 

已 知 旋转 轴 和 旋转 的 度数 ， 计 
算 对 应 的 旋转 四 元 式 
Quaternion(const tfScalar& yaw, const tfScalar& pitch, const| 已 知 欧 拉 旋转 的 三 个 角度 it 


] setRotation(const Vector3& axis, const tfScalar& angle) 





2 P mus iti 
tfScalar& roll) 算 对 应 的 旋转 四 元 式 
3 Quaternion inverse() const 计算 逆 旋 转 的 旋转 四 元 式 
4 Quaternion operator+(const Quaternion& q2) 计算 两 个 旋转 于 加 的 旋转 四 元 式 
5 Quaternion operator*(const Quaternion& ql, const Quaternion& q2) 计算 两 个 旋转 的 迭代 





CE 
利用 球面 插值 得 到 介 于 两 个 已 

知 旋转 之 间 的 近似 值 

8 Vector3 getAxis() 利用 四 元 式 得 到 旋转 的 轴 
计算 一 个 向 量 在 旋转 之 前 的 炎 

值 ， 用 于 旋转 和 平移 的 迭代 


7 slerp(const Quaternion&ql, const quaternion& q2, const tfScalar& t) 


9 Vector3 quatRotate(const Quaternion& rotation, const Vector3& v) 





除了 表 4-3 中 所 列 的 函数 之 外 ，Quaternion 类 中 还 定义 了 计算 四 元 式 的 点 积 、 计 算 两 个 旋转 之 间 的 差 等 相关 阔 数 。 利 用 上 述 函 数 ，tf 可 以 解决 与 旋转 相关 的 问题 ， 例 如 ， 从 A 坐 标 系 到 B 坐 标 系 有 一 次 旋 
转 ， 然 后 B 又 旋转 一 次 得 到 C 坐 标 系 ， 要 求 计算 A 到 C 之 间 的 旋转 关系 ， 此 时 可 以 利用 旋转 的 迭代 函数 去 得 到 所 需 的 结果 。 


除了 利用 四 元 式 表 示 一 个 纯 旋转 ，tf 中 还 定义 了 另外 一 种 用 3x3 和 矩 阵 表示 旋转 的 方法 。 
(3) 3x3 的 旋转 矩 阵 一 一 Matrix3x3 


在 tf 功能 包 的 头 文件 Matrix3x3.h 中 定义 了 Matrix3x3 类 ， 它 用 来 表示 三 维 空 间 中 的 旋转 。 它 定义 的 主要 成 员 变 量 如 下 : 











Vector3 m el[3]; // 存储 矩阵 中 的 数据 ， 每 一 个 Vector3 变 量 代表 矩阵 的 一 行 











tf 定义 旋转 矩阵 的 主要 目的 是 便于 利用 矩阵 的 乘法 ， 从 源 数 据 计 算得 到 旋转 之 后 的 目标 数据 。 为 了 处 理 旋转 矩阵 的 相关 数据 ，Matrix3x3 类 定义 了 相应 的 函数 ， 其 中 重要 的 函数 如 表 4-4 所 示 。 


表 4-4 Mattrix3x3 类 中 定义 的 主要 函数 


编号 函数 名 称 函数 功能 
] | void setRotation(const Quaternion& q) 利用 旋转 四 元 式 得 到 一 个 旋转 矩阵 


利用 欧 拉 旋转 的 三 个 角度 得 到 对 应 





2 | void setEulerYPR(tfScalar eulerZ, tfScalar eulerY,tfScalar eulerX) mera. pea 
的 旋转 矩阵 
3 | void getRotation(Quaternion& q) 利用 旋转 矩阵 得 到 旋转 四 元 式 
利用 旋转 和 矩阵 得 到 欧 拉 旋转 的 三 个 


void getEulerYPR(tfScalar& yaw, tfScalar& pitch,tfScalar& roll) 角度 
又 


Matrix3x3 operator*=(const Matrix3x3& m) 定义 两 个 矩阵 的 乘法 运算 
Matr1x3x3 transpose() 计算 矩阵 的 转 置 

Matrix3x3 inverse() 计算 和 矩阵 的 逆 

Vector3 operator*(const Matrix3x3& m, const Vector3& v) rE SL 3 x 3 FEMA = FE [oy te AY Fe 








BRS 24-4 PATFIASAEASN, Matrix3x38 3 PEM SSR FS, TAS, MUA, RES, MSR, BACB T MISA. 


从 本 质 上 来 说 ，Quaternion 类 和 Matrix3x3 都 可 以 用 来 表示 旋转 ， 在 实际 运用 中 还 会 用 到 欧 拉 旋 转 ， 在 tf 中 定义 了 它们 之 间 的 相互 转换 关系 ， 如 图 4-9 所 示 。 


q.setRotation | 3 





q.setRotation q.setEuler . 







IERES x HMA 






m.setEulerY PR 









m.setEulerY PR 


图 4-9 ”三 种 表示 旋转 的 不 同方 式 之 间 的 相互 转换 关系 


如 图 4-9 所 示 ，q 和 m 分 别 表 示 一 个 四 元 式 的 类 对 象 科 3x3 甜 阵 的 类 对 象 。 图 4-9 中 的 实 线 表 示 在 tf 中 给 出 了 它们 之 间 的 转换 关系 ， 实 线 箭头 上 的 函数 表示 在 转换 中 调用 的 具体 浮 数 。 目 前 tf 尚未 给 出 旋转 
四 元 式 到 欧 拉 旋转 之 间 的 转换 天 系 。 


前 文 已 经 论述 过 ，Quaternion 的 优点 是 便于 插值 。 而 旋转 矩 阵 的 优点 则 是 计算 方便 ， 利 用 和 矩 阵 乘法 ， 我 们 很 容易 就 能 得 到 想 要 的 数据 ， 但 它 的 缺点 刚好 就 是 不 适合 插值 。 欧 拉 旋 转 描述 的 特点 是 更 加 直 
接 ， 易 被 人 理解 。 三 者 同时 存在 并 且 存 在 相互 转换 的 关系 ， 使 得 tf 可 以 根据 不 同 的 情况 选择 不 同 的 表示 方式 ， 以 便 更 好 、 更 快 、 更 准确 地 处 理 实际 的 问题 


2. 与 转换 相关 的 数据 结构 及 其 基本 函数 
(1) 齐 次 坐标 变化 一 一 Transform 


在 tf 功能 包 的 头 文件 Transform.h 中 定义 了 Transform 类 ， 它 用 来 表示 只 包括 旋转 和 平移 (没有 缩放 和 修剪 ) 的 变化 。 在 该 类 中 定义 的 主要 成 员 变 量 为 : 





Matrix3x3 m basis; 
Vector3 m origin; 





另外 一 个 成 员 变 量 m_origin 是 一 个 向 量 ， 表 示 一 个 坐标 原点 和 另外 一 个 坐标 系 原 


其 中 矩阵 m_basis 表 示 坐 标 系 之 间 的 一 个 旋转 关系 ， 这 个 旋转 和 矩阵 可 由 旋转 四 元 式 或 欧 拉 旋转 的 三 个 角度 值 计算 得 来 。 
点 之 间 的 位 移 关 系 。 图 4-10 和 图 4-11 分 别 展 示 了 这 几 种 转换 关系 。 


A 


图 4-10 坐标 系 之 间 的 平移 关系 


图 4-11 坐标 系 之 间 的 平移 和 旋转 关系 


Transform 类 包含 以 下 两 个 重要 的 成 员 函 数 : 


// 计算 tl1,t2 的 迭代 变换 
// 返回 当前 变换 的 逆 变 换 








form& tl, const Transform t2) 








void mult (const Transi 
Transform inverse () 








利用 上 述 函数 ，tf 可 以 很 容易 地 解决 下 面 的 问题 ， 如 图 4-12 所 示 。 


ee X 
ee 
A an transtorm3 
X | transform] 
transform? 


Y 


图 4-12 ”坐标 转换 中 的 迭代 关系 
已 知 坐标 系 A 到 坐标 系 B 的 转换 为 transform1， 以 及 坐标 系 B 到 坐标 系 C 的 转换 为 transform2， 此 时 利用 mult 函 数 可 以 计算 出 A 到 (C 的 转换 : 


transform3=transform! *transform2 


除了 上 述 两 个 函数 之 外 ，Transform 类 中 还 定义 了 与 它 的 成 员 变 量 相关 的 一 些 国 数 。 例 如 ， 已 知 一 个 变化 ， 求 其 中 的 旋转 关系 ， 或 者 是 平移 关系 。 反 之 ， 给 出 两 个 坐标 系 之 间 的 旋转 角度 和 平移 向 量 也 
能 构建 一 个 标准 的 Transform 对 象 。 


(2) 三 维 坐标 系 中 对 象 的 类 模板 一 stamped 


在 tf 功能 包 的 头 文件 transform_datatypes.h 中 定义 了 该 类 模板 ， 它 用 来 泛 指 三 维 空间 中 的 一 个 点 、 一 个 向 量 或 一 个 位 姿 。 它 的 两 个 成 员 变 量 为 : 
ros::Time stamp ; // 与 这 个 数据 对 象 对 应 的 时 间 惟 





std::string frame id; // 与 这 个 数据 对 象 对 应 的 坐标 系 


stamped 类 模板 当中 一 个 重要 的 函数 如 下 : 








void setData(const T& input) // 填充 具体 的 数据 对 象 对 应 的 相关 参数 











stamped 类 主要 用 于 数据 转换 函数 的 参数 和 消息 的 通信 ， 利 用 模板 的 设计 ， 可 以 使 后 续 设 计 转 化 为 函数 更 加 简洁 ， 缩 减 代 码 量 ， 从 而 提升 系统 的 效率 。 


(3) 齐 次 变 














在 tf 功能 包 的 头 文件 time cache.h 中 定义 了 Transformstorage 类 ， 它 定义 了 齐 次 变化 在 系统 中 的 存储 结构 ， 主 要 成 员 变 量 如 下 : 




































































tf::Quaternion rotation ; // 两 个 坐标 系 之 间 的 旋转 四 元 式 

cf: :Vector3 translation ; // 两 个 坐标 系 原点 平移 的 向 量 

ros::Time stamp ; // 与 这 个 彰 次 变换 相关 联 的 时 间 蕉 
CompactFrameID frame id ; // 齐 次 变换 中 的 父亲 坐标 系 ID， 指 的 是 初始 坐标 系 
CompactFrameID child frame id ;// 齐 次 变换 中 的 子 坐标 系 ID， 指 的 是 转换 到 的 坐标 系 
































利用 Transformstorage 类 ， 可 以 轻松 建立 坐标 系 转换 之 间 的 树 形 结构 ， 下 面 是 一 个 简单 的 例子 。 


假设 在 系统 当中 同时 存在 三 个 TransformStorage 的 对 象 ， 分 别 是 A、B 和 CC 并且 A、B、C 满 足下 面 的 条 件 : 








A.frame id =1,A.child frame id =2 
.frame id = 1.8. child frame id =3 
C.frame id =2,C.child frame id =4 





wW 


























根据 A、B、C 的 成 员 变 量 ， 可 以 得 到 坐标 系 1、2、3、4， 满足 如 图 4-13 所 示 的 关系 。 
通过 对 图 4-13 所 示 结 构 树 的 遍历 ，tf 可 以 获取 1、2、3、4 号 坐标 系 任意 两 者 之 间 的 转化 关系 。 关 于 具体 的 遍历 算法 ， 本 书 将 在 后 续 进行 详细 的 介绍 。 


除了 增加 齐 次 转换 对 应 的 父子 坐标 系 之 外 ，Transformstorage 类 当中 还 增加 了 时 间 戳 成 员 变 量 ， 时 间 戳 的 作用 是 区 分 不 同时 刻 两 个 坐标 系 之 间 不 同 的 转换 关系 ， 如 图 4-14 所 示 ， 在 past 时 刻 有 一 个 数 
据 从 坐标 系 turtle1 转 换 到 坐标 系 world; 而 在 now 这 个 时 刻 ， 又 有 一 个 数据 从 坐标 系 world 转 换 到 turtle2 上 ， 在 此 转换 过 程 当 中 ， 时 间 参 数 就 起 到 了 非常 重要 的 作用 ， 它 可 以 区 分 不 同时 刻 的 转换 数据 。 从 
整体 上 来 看 ，tf 功 能 包 的 主要 功能 是 让 用 户 可 以 跟随 时 间 的 变化 跟踪 多 个 不 同 的 坐标 系 。 时 间 上 的 要 求 使 得 时 间 截 成 员 变 量 必须 存在 ， 并 且 是 一 个 非常 重要 的 部 分 ， 在 其 他 部 分 的 一 些 设计 当中 ， 时 间 参 数 
也 有 着 非常 重要 的 作用 。 





图 4-13 ”坐标 系 树 形 结构 图 


/turle | /turle? 








‘turle | /turle? 


/world 一 一 /world 


图 4-14 ”坐标 系 转换 与 时 间 的 关系 图 


(4) 坐标 转换 信息 的 消息 类 一 一 StampedTransform 
在 tf 功能 包 的 头 文件 transform_datatypes.h 中 定义 了 该 类 ， 它 是 Transform 类 的 一 个 子 类 。 它 与 TransformStorage 类 的 设计 类 似 ， 在 Transform 的 基础 上 增加 了 以 下 几 个 成 员 变 量 : 





ros::Time stamp ; // UR FEAR ALT DY HT TH ER 
std::string frame id ; // 与 这 次 转换 相对 应 的 父亲 坐标 系 的 名 称 
// 与 这 换 次 转换 相对 应 的 子 坐标 系 的 名 称 


std::string child frame id ; 


























从 TransformStorage 和 StampedTransform 新 增 的 成 员 变 量 的 不 同 点 来 看 ，Stamped-Transform 表 示 转 换 关系 的 父子 坐标 系 是 它们 的 名 称 ， 而 在 TransformStorage 中 使 用 的 是 坐标 系 内 部 存储 的 
ID， 导 致 这 个 差异 的 原因 正 是 这 两 个 类 的 用 途 不 同 ， 前 文 已 经 介绍 过 ，TransformStorage 是 用 于 转化 数据 的 内 部 存储 ， 而 Stamped-Transform 更 多 的 是 用 作 消 息 类 型 。 在 具体 节点 调用 tf 功能 包 中 的 有 关 
模块 与 其 他 节点 进行 通信 时 ，StampedTransform 往 往 作 为 其 中 的 消息 类 型 进行 传输 。 


(5) 坐标 系 转换 类 一 Transformer 


在 tf 功能 包 的 头 文件 tf.h 中 定义 了 Transformer 的 基本 结构 和 框架 ， 在 源 文 件 tf.cpp 中 对 具体 的 功能 模块 进行 了 具体 实现 。 由 于 Transformer 类 中 的 功能 模块 是 整个 tf 中 较为 核心 的 部 分 ， 因 此 本 章 只 列举 
它 的 模块 和 结构 ， 功 能 模块 的 具体 实现 将 在 后 续 的 章节 中 单独 讲解 。 首 先 ，Transformer 类 中 所 定义 重要 成 员 变 量 如 下 : 








static const unsigned int MAX GRAPH DEPTH = 100UL; 
static const double DEFAULT CACHE TIME; 
static const int64 t DEFAULT MAX EXTRAPOLATION DISTANCE= OULL 
























































其 中 ， 成 员 变量 MAX_GRAPH_DEPTH 的 作用 是 设 定 了 一 个 在 递归 搜索 坐标 系 树 形 结构 时 最 大 深度 ， 这 样 的 设置 是 为 了 防止 树 形 结构 中 可 能 出 现 的 环 状 异常 ， 防 止 程序 出 现 死 循环 的 现象 。 成 员 变 量 
DEFAULT CACHE _ TIME 用 于 设置 一 个 默认 的 发 布 坐标 系 转换 的 频率 。 而 成 员 变量 DEFAULT_ MAX_EXTR-APOLATION _DISTANCE 定 义 的 是 一 个 外 插值 的 默认 最 大 间隔 ， 用 于 在 插值 时 使 用 。 


除了 这 几 个 主要 的 成 员 变 量 之 外 ，Transformer 类 当中 还 定义 了 如 表 4-5 所 示 的 重要 的 成 员 函 数 。 


表 4-5 Transformer # + 19 Bm HH A 


编号 函数 名 称 函数 功能 

添加 一 个 新 的 坐标 系 转化 信息 
bool Transformer::setTransform(const StampedTransform& transform, | 到 特定 的 数据 结构 当中 ， 者 沭 

const std::string& authority) 加 成 功 则 返回 true 值 ， 反 之 为 


false 


void lookupTransform(const std::string& target frame, const 


寻找 在 给 定时 刻 ， 源 坐标 系 和 


2 |std::string& source frame, aoe ; are 
E 目标 坐标 系 之 间 的 坐标 转换 关系 


const ros::Time& time, StampedTransform& transform) 


bool canTransform(const std::string& target frame, const std::string& | 判断 在 给 定时 刻 ， 源 坐标 系 和 
3 |source frame,const ros::Time& time, 目标 坐标 系 是 否 存 在 可 行 的 转换 
std::string* error msg = NULL) 关系 


bool waitForTransform(const std::string& target frame, const std:: 
string& source frame, 每 隅 一 段 时 间 间 隅 检测 在 给 定 
4 const ros::Time& time, const ros::Duration& timeout,const ros:: | 时刻 两 个 坐标 系 之 间 的 转化 是 否 
Duration& polling sleep duration = ros::Duration(0.01), std::string* | 存在 ， 并 且 会 处 于 可 阻塞 的 状态 
error msg = NULL) 


检测 一 个 坐标 系 是 否 在 坐标 系 


5 bool frameExists(const std::string& frame id str) 树 形 结构 当中 
r bool getParent(const std::string& frame id, ros::Time time, std::| 得 到 给 定 坐 标 系 在 树 形 结构 中 
string& parent) 的 父亲 坐标 系 
void transformX(const std::string& target frame, const Stamped 计算 给 定数 据 天 型 在 Ate Bn 
í <X>& stamped_in, Stamped<X>& stamped_out) e 二 site sa Peds a PIIRRE 
据 类 型 ， 如 点 、 回 量 、 旋 转 等 
8 std::string allFramesAsString() Bt D AAE E PRE Se HY 


坐标 系 名 称 





表 4-5 所 列举 出 的 函数 中 ， 编 号 为 2、3、4 的 函数 存在 与 其 功能 相同 但 是 参数 更 加 复杂 的 同名 函数 。 对 于 冰 数 2 而 言 ， 它 的 复杂 形式 的 同名 函数 为 : 











void lookupTransform(const std::string& target frame, const ros::Time& target time , 
const std::string& source frame, const ros::Time& source time,const std:: 
string& fixed frame, StampedTransform& transform) 





























它 与 函数 2 的 区 别 在 于 多 了 一 个 参数 fixed_ frame. fixed framezm—AEN ink, SAAS R, SARE THEN FAM, SKE mente mA Zia 
的 转换 关系 。 从 操作 上 看 ， 先 查找 源 坐 标 系 到 固定 坐标 系 的 转换 ， 再 查找 固定 坐标 系 到 目标 坐标 系 的 变化 ， 然 后 将 两 次 坐标 系 转换 相 乘 即 可 得 到 源 坐标 系 和 目标 坐标 系 之 间 的 转换 关系 。 函 数 3 和 函数 4 的 复 
杂 形 式 的 同名 函数 原理 与 函数 2 的 原理 是 类 似 的 。 


Transformer 类 一 般 不 会 单独 使 用 ， 其 设计 的 主要 目的 是 为 特定 的 功能 模块 提供 相应 的 函数 功能 。 发 布 、 监 听 坐 标 系 变化 都 有 独立 的 模块 ， 这 些 模块 当中 都 需要 调用 Tran-sformer 类 中 的 相关 函数 。 在 
Transformer 类 中 可 以 看 到 很 多 函数 都 使 用 到 time 参 数 ， 下 面 ， 本 书 将 介绍 一 个 与 时 间 相 关 的 类 。 


(6) 与 时 间 相 关 的 坐标 系 转换 关系 一 一 Time_Cache 


在 tf 功能 包 的 头 文件 time_cache.h 和 源 文件 cache.cpp 中 定义 了 Time_Cache 类 。 该 类 的 主要 功能 是 将 坐标 系 之 间 的 转换 与 时 间 紧 密 地 联系 起 来 。 可 以 这 样 理解 该 类 ， 它 利用 特殊 的 结构 将 一 个 坐标 系 与 


另外 一 个 坐标 系 在 不 同时 刻 的 转换 关系 存储 起 来 ， 并 且 提 供 了 相应 的 函数 来 操作 这 些 数 据 。 本 文 之 前 的 论述 中 已 经 提 到 过 ，TransformsSstorage 主 要 用 于 转换 关系 的 人 存储， 在 Time_ Cache 类 中 ， 它 有 着 重要 
的 作用 。 首 先 Time Cache 当中 最 重要 的 一 个 成 员 变量 如 下 : 














typedef std::set<TransformStorage> L TransformStorage; 
L TransformStorage storage ; 




















其 中 storage_ 中 存放 的 每 一 个 TransformStorage 对 象 都 和 有 一 个 共同 的 特点 ， 它 们 对 应 转换 的 父 坐 标 系 和 子 坐 标 系 都 是 同一 个 ， 不 同 之 处 在 于 转换 所 对 应 的 时 刻 不 一 致 。 有 些 节 点 会 以 一 定 的 频率 不 断 地 
发 布 坐 标 转 换 关系 ， 正 因为 如 此 ， 需 要 记录 不 同时 刻 的 坐标 转换 关系 。 由 此 可 以 推 知 ， 每 一 个 Time_Cache 对 象 存储 的 都 是 一 对 坐标 系 在 不 同时 刻 的 转换 关系 。 由 于 tf 中 的 坐标 系 之 间 是 以 树 形 结构 组 织 的 ， 
每 一 个 坐标 系 只 有 一 个 父 坐 标 系 ， 具 有 了 唯一 性 ， 所 以 Time_Cache 类 的 设计 思想 是 ， 每 一 个 坐标 系 都 对 应 一 个 Time_Cache 对 象 ( 根 节点 除外 ， 它 用 0 表示 ) ， 而 该 对 象 存 储 了 它 与 它 的 父 坐 标 系 之 间 在 不 同 
时 刻 的 转化 关系 。 正 因为 这 个 特点 的 存 企 ， 通 过 对 Time_Cache 对 象 的 访问 ，tf 可 以 获取 坐标 系 之 间 的 父子 关系 ， 同 时 也 可 以 完成 系统 中 整个 坐标 系 树 的 遍历 。 此 功能 是 实现 查找 在 任意 时 刻 任意 两 个 坐标 系 
之 间 的 转换 的 前 提 和 基础 。Time_Cache 类 中 定义 了 很 多 与 时 间 相 关 的 浮 数 ， 并 且 也 是 整个 系统 的 核心 部 分 ， 具 体 的 实现 细节 ， 将 在 后 续 章 节 中 详细 介绍 ， 本 节 只 列举 出 函数 的 名 称 和 基本 功能 ， 具 体 遂 数 
列表 如 表 4-6 所 示 。 


#4-6 Time_Cache 中 定义 的 与 时 间 相 关 的 函数 


编写 PR BN BR 函数 


| 

= 
a> 
rer 


uint8 t findClosest(const TransformStorage*& one, const Trans-| 查找 最 接近 给 定时 刻 的 两 个 转 
formStorage*& two, ros::Time target tıme, std::string* error str) 换 关 系数 据 
bool getData(ros::Time time, TransformStorage & data out, std:: 得 到 在 给 定时 刻 两 个 坐标 系 之 
”| string* error str) 间 的 转换 关系 效 据 
my | l 利用 两 个 对 应 时 刻 接 近 的 坐标 
void interpolate(const TransformStorage& one, const Transform- | _— EEH 六 i buaa 
3 o n i 系 转换 数据 ， 利 用 插值 得 到 近似 
Storage& two, ros::Tıme time, TransformStorage& output) pia 
的 转换 数据 
i 锋 取 在 给 定时 刻 坐 标 系 转 化 关系 
| CompactFrameID getParent(ros::Time time, std::string* error str) f 


中 的 父亲 坐标 系 ID ,不 存在 就 取 0 
5 bool insertData(const TransformStorage& new data) 插入 一 次 新 的 坐标 系 转化 数据 
删除 存储 时 间 超过 最 大 存在 时 


void pruneList() 间 间 隔 的 坐标 系 转化 数据 





在 有 了 表 4-6 所 列 的 函数 的 基础 上 ，tf 在 对 Time_Cache 具 体 对 象 进行 操作 和 访问 时 ， 可 以 获取 一 个 坐标 系 和 它 的 父 坐 标 系 在 给 定时 间 范 围 内 任意 时 刻 的 坐标 系 转换 数据 ， 同 时 也 可 以 添加 新 的 转换 数据 
并 且 删 除 已 经 过 时 的 数据 。 以 上 功能 的 实现 都 是 使 得 tf 能 跟踪 任意 时 刻 两 个 坐标 系 转换 的 必要 设计 。 为 了 方便 在 特定 的 时 刻 获取 相应 的 数据 ，Time_Cache 类 中 还 定义 了 直接 获取 最 新 时 刻 的 和 最 早 时 刻 的 转 
换 数 据 的 函数 ， 此 设计 也 在 一 定 程 度 上 提升 了 系统 的 效率 。 除 此 之 外 ，Time_Cache 类 中 还 定义 了 三 种 插值 可 能 出 现 异 常 的 情况 ， 它 们 分 别 如 下 。 


只 有 一 个 数据 在 缓存 当中 无 法 进行 插值 。 
需要 进行 播 值 的 数据 对 应 的 时 刻 最 晚 ， 没 有 更 晚 的 数据 可 用 于 播 值 。 
需要 进行 播 值 的 数据 对 应 的 时 刻 最 旱 ， 没 有 更 早 的 数据 可 用 于 插值 。 


通过 对 不 同 插值 异常 情况 的 报告 ， 系 统 可 以 清楚 地 给 出 相应 的 错误 信息 ， 使 用 户 可 以 根据 相应 的 错误 信息 ， 做 出 相应 的 调整 。 总 的 来 说 ， 对 该 类 有 个 好 的 设计 是 整个 tf 系统 提升 与 时 间 相关 的 坐标 系 转 
换 的 计算 效率 的 重要 举措 之 一 。 


(7) 异常 类 一 一 Exception 


在 tf 处 理 数据 的 过 程 中 ， 在 不 同 的 阶段 和 不 同 的 模块 当中 ， 不 可 避免 地 会 出 现 各 种 各 样 的 异常 。 通 过 对 异常 进行 记录 和 处 理 ， 可 以 让 tf 更 好 地 掌控 各 个 模块 的 运行 状态 ， 这 对 维持 整个 系统 会 有 很 重要 的 
作用 。tf 中 定义 的 异常 如 表 4-7 所 示 。 


RAT tf 中 定义 的 异常 


编号 具体 含义 

| 在 结构 树 上 找 不 到 指定 的 坐标 系 

2 在 需要 进行 插值 计算 时 ， 数 据 出 现 异 党 
3 基本 数据 结构 中 的 参数 不 满足 条 件 

4 可 阻塞 的 函数 中 出 现 超时 的 情况 


在 查找 两 个 坐标 系 之 间 的 转化 关系 时 ， 因 两 个 坐标 系 不 
在 同一 棵 坐标 系 树 上 的 异常 


AN 


ConnectivityException 





4.3 tf 原理 分 析 


4.3.1 ”相关 数据 结构 


ROS tf 为 了 实现 坐标 变换 ， 设 计 了 相关 数据 结构 ， 描 述 坐 标 系 及 其 转换 关系 。 为 了 深入 分 析 这 些 数据 结构 的 组 成 及 其 作用 ， 我 们 将 其 分 为 数学 相关 和 转换 相关 两 类 数据 结构 。 
1. 数 学 相关 的 数据 结构 及 其 基本 逊 数 
(1) 空间 点 和 空间 向 量 描述 一 一 Vector3 


在 tf 功能 包 的 头 文件 Vector3.h 中 定义 了 Vector3 类 ， 用 来 表示 三 维 空间 的 一 个 点 或 一 个 向 量 。Vertor3 定 义 的 一 个 主要 成 员 变 量 为 : 














tfScalarm floats[4]; // tfScalar 相当 于 double 





这 里 用 x、y、z、w 分 别 来 表示 m floats[0]、m floats[1]、m floats[2]、m floats[3]。 
其 中 x、y、z 对 应 三 维 空间 的 三 个 坐标 参数 ， 而 w 则 用 于 控制 数据 是 否 对 齐 ， 正 常情 况 下 为 0.0。 


成 员 变 量 m_floats 表 示 一 进行 个 向 量 或 一 个 点 。 对 于 一 个 点 来 说， 独立 计算 比较 少 ， 后 文 将 结合 齐 次 变换 再 进行 一 定 说 明 ， 这 里 主要 介绍 向 量 处 理 及 其 计算 。 向 量 主要 用 于 表示 坐标 系 转化 中 坐标 原点 


之 间 的 平移 关系 。 


Vector3 中 定义 了 一 组 与 向 量 相关 的 操作 函数 ， 具 体 的 遂 数 如 表 4-2 所 示 。 


表 4-2  Vectot3 类 中 定义 的 主要 函数 


PETZ BR 项 数 功 能 


] tfScalar tfDot(const Vector3& vl, const Vector3& v2) 计算 两 个 回 量 的 点 积 (内 积 ) 


2 tfScalar length() ITF [a] ATR EL) 
3 Vector3& normalize() 求 与 已 知 回 量 同 回 的 单位 回 量 





1 tfScalartfAngle(const Vector3& vl, const Vector3& v2) 计算 两 个 回 量 的 夹 角 
5 tfScalartfDistance(const Vector3& vl, const Vector3v2) 计算 两 个 回 量 之 间 的 距离 
6 Vector3 tfCross(const Vector3& vl, const Vector3& v2) 计算 .两 个 回 量 的 叉 积 (外 积 ) 
void setInterpolate3(const Vector3& v0, const Vector3& vl,| 利用 已 知 的 两 个 回 量 线性 捕 值 得 到 介 
tfScalar rt) 于 两 者 之 间 的 一 个 近似 回 量 
| 、 计算 指定 癌 量 绕 给 定单 位 问 量 旋转 角 
8 Vector3 rotate( const Vector3& wAxis, const tfScalar angle ) | .. or äi 7 am “— ° 
¥ angle 之 后 得 到 的 癌 量 





除了 表 4-2 中 所 列 的 函数 之 外 ，tf 还 定义 了 一 些 向 量 基 本 运算 的 函数 ， 如 向 量 之 间 的 加 关系 、 减 天 系 、 乘 关系 (不 同 于 内 积 和 叉 积 ) 、 除 天 系 等 。 利 用 这 些 函 数 及 表格 中 所 述 的 函数 ，tf 可 以 很 好 地 处 理 


不 同 向 量 之 间 的 关系 。 已 知 两 个 向 量 的 坐标 ， 可 以 利用 表格 中 的 相关 函数 计算 得 到 它们 之 间 的 夹 角 和 距离 ， 以 便于 后 续 操作 的 继续 进行 ， 进 行 旋转 或 平移 。 


Vector3 中 定义 的 成 员 变量 m_floats 不 仅 可 以 独立 表示 一 个 向 量 或 是 一 个 点 ， 同 时 在 定义 后 述 的 Matrix3x3 (表示 一 个 3x3 和 矩 阵 ) 时 ， 它 也 可 以 作为 Matrix3x3 的 成 员 变 量 中 的 一 行 。 在 处 理 Matrix3x3 的 


相关 计算 时 ， 把 其 中 的 一 行当 成 一 个 整体 并 且 利 用 Vector3 中 定义 的 向 量 处 理 函 数 ， 也 能 使 处 理 数 据 变 得 更 加 有 效率 ， 这 也 说 明 Vector3 类 一 个 好 的 设计 对 提升 整个 系统 的 计算 效率 是 很 有 必要 的 。 





在 tf 功能 包 的 头 文 件 Quaternion.h 中 定义 了 Quaternion 类 ， 它 用 来 表示 在 三 维 空间 当中 的 一 次 纯 旋转 。Quaternion 类 继承 于 它 的 父 类 QuadWord。 在 QuadWord 类 当中 主要 定义 的 成 员 变 量 如 下 : 














tfScalar m floats[4];  // tfScalar 相当 于 double 


本 节 用 x、y、z、w 分 别 来 表示 m floats[0]、m floats[1]、m floats[2]、m _ floats[3]。 一 般 来 说 ， 有 旋转 矩阵 、 欧 拉 旋 转 及 四 元 数 三 种 方式 表示 旋转 。 


旋转 矩阵 是 使 用 一 个 特定 的 矩阵 来 表示 绕 任 意 轴 旋转 。 欧 拉 旋 转 则 是 通过 定义 坐标 轴 顺 序 (例如 先 x、 再 y、 最 后 z) 和 绕 每 个 轴 旋 转 的 角度 来 表示 一 次 旋转 ， 实 际 上 是 一 系列 坐标 轴 旋 转 的 组 合 。 


四 元 数 本 质 上 是 一 种 高 阶 复数 ， 是 一 个 四 维 空间 ， 相 当 于 复数 的 二 维 空间 。 复 数 由 实 部 和 虚 部 组 成 ， 即 x=a+bi (RAW, i°=-1) 。 四 元 数 和 复数 的 定义 是 类 似 的 ， 不 同 的 是 ， 它 的 虚 部 包含 了 三 


个 虚数 单位 ，i、j、k， 即 四 元 数 可 以 表示 为 Q=w+xi+yj+zk。QuadWord 类 当中 的 成 员 变 量 m floats 的 4 个 变量 分 别 对 应 这 里 的 x，y，z，w。 结 合 具体 的 例子 来 说 ， 可 以 这 样 理解 一 个 四 元 数 。 给 定 一 个 单 
位 长 度 的 旋转 轴 (a, b, c) 和 一 个 角度 6， 对 应 的 四 元 数 为 : 


q= (a*sin (0/2) , b*sin (0/2) , c*sin (0/2) , cos (0/2) ) 


三 种 旋转 的 表示 方法 中 ， 每 种 方法 都 有 各 自 的 优 缺 点 。 其 中 四 元 式 表示 方法 有 一 个 重要 的 优点 是 可 以 提供 平滑 的 插值 ， 这 是 ROS 所 必要 且 特 有 的 ， 在 后 续 的 论述 中 会 对 其 的 必要 性 作 相 应 的 解释 。 


QuadWord 类 中 仅仅 定义 了 主要 的 成 员 变量 ， 而 Quaternion 类 中 则 定义 了 四 元 式 的 相关 操作 函数 ， 具 体 函 数 如 表 4-3 所 示 。 


表 4-3 ”Quatetnion 类 中 定义 的 主要 函数 


函数 功能 





已 知 旋 转轴 和 旋转 的 度数 ， 计 
setRotation(const Vector3& axis, const tfScalar& angle) rmn aan 
算 对 应 的 旋转 四 元 式 
Quaternion(const tfScalar& yaw, const tfScalar& pitch, const| CARH ERAN => ARE. it 
tfScalar& roll) 算 对 应 的 旋转 四 元 式 
a RENEE 
4 Quaternion operator+(const Quaternion& q2) ITF PT eee I EFS UJC 
5 Quaternion operator*(const Quaternion& ql, const Quaternion& q2) 计算 Ara = 迭代 





CE 
利用 球面 插值 得 到 介 于 两 个 已 
知 旋 转 之 间 的 近似 值 
8 Vector3 getAxis() 利用 四 元 式 得 到 旋转 的 轴 
计算 一 个 向 量 在 旋转 之 前 的 数 
值 ， 用 于 旋转 和 平移 的 迭代 


7 slerp(const Quaternion&q1, const quaternion& q2, const tfScalar& t) 





9 Vector3 quatRotate(const Quaternion& rotation, const Vector3& v) 





除了 表 4-3 中 所 列 的 函数 之 外 ，Quaternion 类 中 还 定义 了 计算 四 元 式 的 点 积 、 计 算 两 个 旋转 之 间 的 差 等 相关 阔 数 。 利 用 上 述 函 数 ，tf 可 以 解决 与 旋转 相关 的 问题 ， 例 如 ， 从 A 坐 标 系 到 B 坐 标 系 有 一 次 旋 
转 ， 然 后 B 又 旋转 一 次 得 到 C 坐 标 系 ， 要 求 计算 A 到 C 之 间 的 旋转 关系 ， 此 时 可 以 利用 旋转 的 迭代 函数 去 得 到 所 需 的 结果 。 


除了 利用 四 元 式 表 示 一 个 纯 旋转 ， 革 中 还 定义 了 另外 一 种 用 3x3 和 矩 阵 表 示 旋 转 的 方法 。 
(3) 3x3 的 旋转 矩 阵 一 一 Matrix3x3 


在 tf 功能 包 的 头 文件 Matrix3x3.h 中 定义 了 Matrix3x3 类 ， 它 用 来 表示 三 维 空 间 中 的 旋转 。 它 定义 的 主要 成 员 变 量 如 下 : 











Vector3 m el[3]; // 存储 和 矩阵 中 的 数据 ， 每 一 个 Vector3 变 量 代表 和 矩阵 的 一 行 











tf 定义 旋转 矩阵 的 主要 目的 是 便于 利用 矩 阵 的 乘法 ， 从 源 数据 计算 得 到 旋转 之 后 的 目标 数据 。 为 了 处 理 旋转 矩阵 的 相关 数据 ，Matrix3x3 类 定义 了 相应 的 函数 ， 其 中 重要 的 函数 如 表 4-4 所 示 。 


表 4-4 Matrix3x3 类 中 定义 的 主要 函数 


编号 函数 功能 
] | void setRotation(const Quaternion& q) 利用 旋转 四 元 式 得 到 一 个 旋转 矩阵 
| 用 欧 拉 


| 诈 转 的 三 个 角度 得 到 对 应 


y 


2 | void setEulerYPR(tfScalar eulerZ, tfScalar eulerY,tfScalar eulerX) 








FY Tie Fe Ht PF 
3 | void getRotation(Quaternion& q) 利用 旋转 矩阵 得 到 旋转 四 元 式 
, , , 利用 旋转 矩阵 得 到 欧 拉 旋 转 的 三 个 
void getEulerYPR(tfScalar& yaw, tfScalar& pitch,tfScalar& roll) 角度 
WS 
Matrix3x3 operator*=(const Matrix3x3& m) 定义 两 个 矩阵 的 乘法 运算 
Matrix3x3 transpose() 计算 矩阵 的 转 置 
Matrix3x3 inverse() 计算 矩阵 的 逆 
Vector3 operator*(const Matrix3x3& m, const Vector3& v) 定义 3x3 和 矩阵 和 三 维 回 量 的 乘法 
除了 表 4-4 中 所 列举 的 函数 ，Matrix3x3 类 当中 还 定义 了 计算 矩 阵 余 子 式 ， 计 算 伴 随和 矩阵 ， 和 矩阵 对 角 化 ， 矩 阵 相 乘 的 转 置 ， 和 转 置 后 相 乘 的 函数 ， 基 本 上 包括 了 对 和 矩 阵 的 所 有 的 操作 。 


从 本 质 上 来 说 ，Quaternion 类 和 Matrix3x3 都 可 以 用 来 表示 旋转 ， 在 实际 运用 中 还 会 用 到 欧 拉 旋 转 ， 在 tf 中 定义 了 它们 之 间 的 相互 转换 关系 ， 如 图 4-9 所 示 。 





旋转 四 元 式 






q.setRotation 





q.setRotation q.setEuler 





ao 欧 拉 旋转 


m.setEulerY PR 


i 
m.setEulerYPR 


B49 ”三 种 表示 旋转 的 不 同方 式 之 间 的 相互 转换 关系 


如 图 4-9 所 示 ，gq 和 m 分 别 表 示 一 个 四 元 式 的 类 对 象 和 3x3 和 矩阵 的 类 对 象 。 图 4-9 中 的 实 线 表 示 在 tf 中 给 出 了 它们 之 间 的 转换 关系 ， 实 线 箭头 上 的 函数 表示 在 转换 中 调用 的 具体 函数 。 目 前 tf 尚未 给 出 旋转 
四 元 式 到 欧 拉 旋转 之 间 的 转换 关系 。 
前 文 已 经 论述 过 ，Quaternion 的 优点 是 便于 插值 。 而 旋转 和 矩 阵 的 优点 则 是 计算 方便 ， 利 用 和 矩 阵 乘法 ， 我 们 很 容易 就 能 得 到 想 要 的 数据 ， 但 它 的 缺点 刚好 就 是 不 适合 插值 。 欧 拉 旋 转 描述 的 特点 是 更 加 直 
接 ， 易 被 人 理解 。 三 者 同时 存在 并 且 存 在 相互 转换 的 关系 ， 使 得 tf 可 以 根据 不 同 的 情况 选择 不 同 的 表示 方式 ， 以 便 更 好 、 更 快 、 更 准确 地 处 理 实际 的 问题 。 
2. 与 转换 相关 的 数据 结构 及 其 基本 逊 数 
(1) 齐 次 坐标 变化 一 一 Transform 


在 tf 功能 包 的 头 文件 Transform.h 中 定义 了 Transform 类 ， 它 用 来 表示 只 包括 旋转 和 平移 (没有 缩放 和 修剪 ) 的 变化 。 在 该 类 中 定义 的 主要 成 员 变 量 为 : 


Matrix3x3 m basis; 
Vector3 m origin; 


O pr- 


Eirhyal4m_basisseasbinAR NERKA, AANER AE ATAARE RENAE RSR. PAoa origina AE, Ra AANRAAI ERRER 
点 之 间 的 位 移 关 系 。 图 4-10 和 图 4-11 分 别 展 示 了 这 几 种 转换 关系 。 


A 


图 4-10 ”坐标 系 之 间 的 平移 关系 


图 4-11 坐标 系 之 间 的 平移 和 旋转 关系 


Transform 类 包含 以 下 两 个 重要 的 成 员 了 数 : 

















void mult (const Transformé tl, const Transform& t2) // 计算 tl,t2 的 迭代 变换 
Transform inverse () // BE 4 AAR E ae 


























利用 上 述 函数 ，tf 可 以 很 容易 地 解决 下 面 的 问题 ， 如 图 4-12 所 示 。 


A a transform3 


X transform] 
transform? 
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图 4-12 ”坐标 转换 中 的 迭代 关系 
已 知 坐标 系 A 到 坐标 系 B 的 转换 为 transform1， 以 及 坐标 系 B 到 坐标 系 C 的 转换 为 transform2， 此 时 利用 mult 函 数 可 以 计算 出 A 到 C 的 转换 : 


transform3=transform! *transform2 


除了 上 述 两 个 函数 之 外 ，Transform 类 中 还 定义 了 与 它 的 成 员 变 量 相关 的 一 些 国 数 。 例 如 ， 已 知 一 个 变化 ， 求 其 中 的 旋转 关系 ， 或 者 是 平移 关系 。 反 之 ， 给 出 两 个 坐标 系 之 间 的 旋转 角度 和 平移 向 量 也 
能 构建 一 个 标准 的 Transform 对 象 。 


(2) 三 维 坐标 系 中 对 象 的 类 模板 一 stamped 


~~ 


a parE. 
员 变 量 为 : 


etfs 


ros::Time stamp ; // GIR AS BE AT BT DOL EIEN TL BK 
std::string frame id; // 与 这 个 数据 对 象 对 应 的 坐标 系 





stamped 类 模板 当中 一 个 重要 的 函数 如 下 : 




















void setData(const T& input) // 填充 具体 的 数据 对 象 对 应 的 相关 参数 


stamped 类 主要 用 于 数据 转换 函数 的 参数 和 消息 的 通信 ， 利 用 模板 的 设计 ， 可 以 使 后 续 设 计 转 化 为 函数 更 加 简洁 ， 缩 减 代 码 量 ， 从 而 提升 系统 的 效率 。 


(3) FF 





在 tf 功能 包 的 头 文件 time_cache.h 中 定义 了 TransformStorage 类 ， 它 定义 了 齐 次 变化 在 系统 中 的 存储 结构 ， 主 要 成 员 变 量 如 下 : 







































































tf::Quaternion rotation ; // BAT obs R e IRO ETE IE 式 

cf: :Vector3 translation ; // 两 个 坐标 系 原点 平移 的 向 量 

ros::Time stamp ; // 与 这 个 齐 次 变换 相关 联 的 时 le) ak 
CompactFrameID frame id ; // 齐 次 变换 中 的 父亲 坐标 系 ID， 指 的 是 初始 坐标 系 
CompactFrameID child f rane gd_;// 齐 次 变换 中 的 子 坐 标 系 ID， 指 的 是 转换 到 的 坐标 系 





利用 Transformstorage 类 ， 可 以 轻松 建立 坐标 系 转换 之 间 的 树 形 结构 ， 下 面 是 一 个 简单 的 例子 。 


假设 在 系统 当中 同时 存在 三 个 Transformstorage 的 对 象 ， 分 别 是 A、B 和 (C 并 且 A、B、C 满 足下 面 的 条 件 : 


A.frame id =1,A.child frame id =2 
.frame id = IB, child frame id =3 
C.frame | id : =2,C.child frame id =4 








wW 


























根据 A、B、C 的 成 员 变 量 ， 可 以 得 到 坐标 系 1、2、3、4， 满足 如 图 4-13 所 示 的 关系 。 
通过 对 图 4-13 所 示 结 构 树 的 遍历 ，tf 可 以 获取 1、2、3、4 号 坐标 系 任意 两 者 之 间 的 转化 关系 。 关 于 具体 的 遍历 算法 ， 本 书 将 在 后 续 进行 详细 的 介 


除了 增加 齐 次 转换 对 应 的 父子 坐标 系 之 外 ，Transformstorage 类 当中 还 增加 了 时 间 戳 成 员 变 量 ， 时 间 戳 的 作用 是 区 分 不 同时 刻 两 个 坐标 系 之 间 不 同 的 转换 关系 ， 如 图 4-14 所 示 ， 在 past 时 刻 有 一 个 数 
据 从 坐标 系 turtle1 转 换 到 坐标 系 world; 而 在 now 这 个 时 刻 ， 又 有 一 个 数据 从 坐标 系 world 转 换 到 turtle2 上 ， 在 此 转换 过 程 当 中 ， 时 间 参 数 就 起 到 了 非常 重要 的 作用 ， 它 可 以 区 分 不 同时 刻 的 转换 数据 。 从 
整体 上 来 看 ，tf 功 能 包 的 主要 功能 是 让 用 户 可 以 跟随 时 间 的 变化 跟踪 多 个 不 同 的 坐标 系 。 时 间 上 的 要 求 使 得 时 间 截 成 员 变 量 必须 存在 ， 并 且 是 一 个 非常 重要 的 部 分 ， 在 其 他 部 分 的 一 些 设计 当中 ， 时 间 参 数 
也 有 着 非常 重要 的 作用 。 





图 4-13 ”坐标 系 树 形 结 构图 
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图 4-14 坐标 系 转换 与 时 间 的 关系 图 


(4) 坐标 转换 信息 的 消息 类 一 StampedTransform 
在 tf 功能 包 的 头 文 件 transform_datatypes.h 中 定义 了 该 类 ， 它 是 Transform 类 的 一 个 子 类 。 它 与 Transformstorage 类 的 设计 类 似 ， 在 Transform 的 基础 上 增加 了 以 下 几 个 成 员 变 量 


ros::Time stamp ; // 与 这 次 转换 相对 应 的 时 间 惟 
std::string frame id ; // 与 这 次 转换 相对 应 的 父亲 坐标 系 的 名 称 
// 与 这 换 次 转换 相对 应 的 子 坐 标 系 的 名 称 


std::string child frame id ; 
































从 TransformStorage 和 StampedTransform 新 增 的 成 员 变 量 的 不 同 点 来 看 ，Stamped-Transform 表 示 转 换 关系 的 父子 坐标 系 是 它们 的 名 称 ， 而 在 TransformStorage 中 使 用 的 是 坐标 系 内 部 存储 的 
类 型 。 在 具体 节点 调用 tf 功能 包 中 的 有 关 


异 的 原因 正 是 这 两 个 类 的 用 途 不 同 ， 前 文 已 经 介绍 过 ，TransformStorage 是 用 于 转化 数据 的 内 部 存储 ， 而 Stamped-Transform 更 多 的 是 用 作 消 息 


ID， 导 致 这 个 
模块 与 其 他 节点 进行 通信 时 ，StampedTransform 往 往 作为 其 中 的 消息 类 型 进行 传输 。 


(5) 坐标 系 转换 类 一 一 Transformer 


在 tf 功能 包 的 头 文件 tf.h 中 定义 了 Transformer 的 基本 结构 和 框架 ， 在 源 文 件 tf.cpp 中 对 具体 的 功能 模块 进行 了 具体 实现 。 


员 变 量 如 下 : 


由 于 Transformer 类 中 的 功能 模块 是 整个 tf 中 较为 核心 的 部 分 ， 因 此 本 章 只 列举 
它 的 模块 和 结构 ， 功 能 模块 的 具体 实现 将 在 后 续 的 章节 中 单独 讲解 。 首 先 ，Transformer 类 中 所 定义 重要 成 


tic const unsigned int MAX GRAPH DEPTH = 100UL; 


tic const double DEFAULT CACHE TIME; 
tic const int64 t DEFAULT MAX EXTRAPOLATION DISTANCE= OULL 
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其 中 ， 成 员 变量 MAX_GRAPH_DEPTH 的 作用 是 设 定 了 一 个 在 递归 搜索 坐标 系 树 形 结构 时 最 大 深度 ， 这 样 的 设置 是 为 了 防止 树 形 结构 中 可 能 出 现 的 环 状 异常 ， 防 止 程序 出 现 死 循环 的 现象 。 成 员 变 量 
DEFAULT CACHE _ TIME 用 于 设置 一 个 默认 的 发 布 坐标 系 转换 的 频率 。 而 成 员 变量 DEFAULT_ MAX_EXTR-APOLATION _DISTANCE 定 义 的 是 一 个 外 插值 的 默认 最 大 间隔 ， 用 于 在 插值 时 使 用 。 
除了 这 几 个 主要 的 成 员 变 量 之 外 ，Transformer 类 当中 还 定义 了 如 表 4-5 所 示 的 重要 的 成 员 函 数 。 
表 4-5 Transformer % + 19 Bm HH A 


函数 功能 
添加 一 个 新 的 坐标 系 转化 信息 
bool Transformer::setTransform(const StampedTransform& transform, | 到 特定 的 数据 结构 当中 ， 者 沭 
const std::string& authority) 加 成 功 则 返回 true 值 ， 反之 为 


false 


void lookupTransform(const std::string& target frame, const 


寻找 在 给 定时 刻 ， 源 坐标 系 和 
目标 坐标 系 之 间 的 坐标 转换 关系 


std::string& source frame, 


const ros::Time& time, StampedTransform& transform) 


bool canTransform(const std::string& target frame, const std::string& | 判断 在 给 定时 刻 ， 源 坐标 系 和 
source _frame,const ros::Time& time, 目标 坐标 系 是 否 存 在 可 行 的 转换 


std::string* error msg = NULL) KA 


bool waitForTransform(const std::string& target frame, const std:: 
string& source frame, f Bs — Bt EY Via] pE Ba BW HE 29 HE 

const ros::Time& time, const ros::Duration& timeout,const ros:: | 时 刻 两 个 坐标 系 之 间 的 转化 是 否 
Duration& polling sleep duration = ros::Duration(0.01), std::string* | 存在， 并且 会 处 于 可 阻塞 的 状态 
error msg = NULL) 

检测 一 个 坐标 系 是 否 在 坐标 系 
峙 形 结构 当中 

bool getParent(const std::string& frame id, ros::Time time, std::| 得 到 给 定 坐 标 系 在 树 形 结构 中 
string& parent) 的 父亲 上 坐标 系 

计算 给 定数 据 类 型 在 目标 坐标 
系 中 的 数据 但 ，X 代表 不 同 的 数 
据 类 型 ， 如 点 、 回 量 、 放 转 等 

查看 所 有 存在 于 树 形 结构 中 的 
坐标 系 名 称 


bool frameExists(const std::string& frame id_str) 


void transformX(const std::string& target frame, const Stamped 


<X>& stamped in, Stamped<X>& stamped _ out) 


std::string allFramesAsString() 





表 4-5 所 列举 出 的 函数 中 ， 编 号 为 2、3、4 的 函数 存在 与 其 功能 相同 但 是 参数 更 加 复杂 的 同名 国 数 。 对 于 函数 2 而 的 复杂 形式 的 同名 函数 为 : 


Lil} 
a} 








void lookupTransform(const std::string& target frame, const ros::Time& target time , 
const std::string& source frame, const ros::Time& source time,const std:: 
string& fixed frame, StampedTransform& transform) 
































它 与 函数 2 的 区 别 在 于 多 了 一 个 参数 fixed_ frame. fixed framexm—AENSinK. SAAS Rh, eRe CHEN SnA FAME, SKE mentee mA ZA 
的 转换 关系 。 从 操作 上 看 ， 先 查找 源 坐 标 系 到 固定 坐标 系 的 转换 ， 再 查找 固定 坐标 系 到 目标 坐标 系 的 变化 ， 然 后 将 两 次 坐标 系 转换 相 乘 即 可 得 到 源 坐标 系 和 目标 坐标 系 之 间 的 转换 关系 。 函 数 3 和 函数 4 的 复 
杂 形 式 的 同名 函数 原理 与 函数 2 的 原理 是 类 似 的 。 


Transformer 类 一 般 不 会 单独 使 用 ， 其 设计 的 主要 目的 是 为 特定 的 功能 模块 提供 相应 的 函数 功能 。 发 布 、 监 听 坐 标 系 变化 都 有 独立 的 模块 ， 这 些 模块 当中 都 需要 调用 Tran-sformer 类 中 的 相关 函数 。 在 
Transformer 类 中 可 以 看 到 很 多 函数 都 使 用 到 time 参 数 ， 下 面 ， 本 书 将 介绍 一 个 与 时 间 相 关 的 类 。 


(6) 与 时 间 相关 的 坐标 系 转换 天 系 一 Time_Cache 


在 tf 功能 包 的 头 文件 time_cache.h 和 源 文 件 cache.cpp 中 定义 了 Time Cache 类。 该 类 的 主要 功能 是 将 坐标 系 之 间 的 转换 与 时 间 紧 密 地 联系 起 来 。 可 以 这 样 理解 该 类 ， 它 利用 特殊 的 结构 将 一 个 坐标 系 与 
另外 一 个 坐标 系 在 不 同时 刻 的 转换 关系 存储 起 来 ， 并 上 且 提供 了 相应 的 函数 来 操作 这 些 数据 。 本 文 之 前 的 论述 中 已 经 提 到 过 ，TransformStorage 主 要 用 于 转换 关系 的 存储 ， 在 Time_Cache 类 中 ， 它 有 着 重要 
的 作用 。 首 先 Time_Cache 当 中 最 重要 的 一 个 成 员 变 量 如 下 : 




















typedef std::set<TransformStorage> L TransformStorage; 
L TransformStorage storage ; 











其 中 storage 中 存放 的 每 一 个 TransformStorage 对 象 都 和 一 个 共同 的 特点 ， 它 们 对 应 转换 的 父 坐 标 系 和 子 坐 标 系 都 是 同一 个 ， 不 同 之 处 在 于 转换 所 对 应 的 时 刻 不 一 致 。 有 些 节 点 会 以 一 定 的 频率 不 断 地 
发 布 坐 标 转换 关系 ， 正 因为 如 此 ， 需 要 记录 不 同时 刻 的 坐标 转换 关系 。 由 此 可 以 推 知 ， 每 一 个 Time_Cache 对 象 存储 的 都 是 一 对 坐标 系 在 不 同时 刻 的 转换 关系 。 由 于 tf 中 的 坐标 系 之 间 是 以 树 形 结构 组 织 的 ， 
每 一 个 坐标 系 只 有 一 个 父 坐 标 系 ， 具 有 了 唯一 性 ， 所 以 Time_Cache 类 的 设计 思想 是 ， 每 一 个 坐标 系 都 对 应 一 个 Time_Cache 对 象 ( 根 节点 除外 ， 它 用 0 表示 ) ， 而 该 对 象 存 储 了 它 与 它 的 父 坐 标 系 之 间 在 不 同 
时 刻 的 转化 关系 。 正 因为 这 个 特点 的 存在 ， 通 过 对 Time_Cache 对 象 的 访问 ，tf 可 以 获取 坐标 系 之 间 的 父子 关系 ， 同 时 也 可 以 完成 系统 中 整个 坐标 系 树 的 遍历 。 此 功能 是 实现 查找 在 任意 时 刻 任意 两 个 坐标 系 
之 间 的 转换 的 前 提 和 基础 。Time_Cache 类 中 定义 了 很 多 与 时 间 相 关 的 函数 ， 并 且 也 是 整个 系统 的 核心 部 分 ， 具 体 的 实现 细节 ， 将 在 后 续 章 节 中 详细 介绍 ， 本 节 只 列举 出 函数 的 名 称 和 基本 功能 ， 具 体 函 数 
列表 如 表 4-6 所 示 。 


#4-6 Time_Cache 中 定义 的 与 时 间 相 关 的 函数 


- 
oS 
a> 
over 


编号 函数 名 称 函数 


i uint8 t findClosest(const TransformStorage*& one, const Trans-| 查找 最 接近 给 定时 刻 的 两 个 
formStorage*& two, ros::Time target time, std::string” error str) — 系数 据 
bool getData(ros::Time time, TransformStorage & data out, std:: 导 到 在 给 定时 刻 两 个 坐标 系 之 
~ |string* error str) [il] PIERE 大 系数 据 
y 、 利用 两 个 对 应 时 刻 接 近 的 坐标 
void interpolate(const TransformStorage& one, const Transform- | _ a : š 
3 a 系 转换 数据 ， 利 用 插值 得 到 近似 
Storage& two, ros::Time time, TransformStorage& output) ie ean 
的 转换 数据 
获取 在 给 定时 刻 坐标 系 转 化 天 系 
1 CompactFramelD getParent(ros:: Time time, std::string* error str) ` 


中 的 父亲 坐标 系 ID ， 不 存在 就 取 0 
5 bool insertData(const TransformStorage& new data) 插入 一 次 新 的 坐标 系 转化 数据 
删除 存储 时 间 超 过 最 大 存在 时 


0 void pruneList() 间 间 隔 的 坐标 系 转化 数据 





在 有 了 表 4-6 所 列 的 遂 数 的 基础 上 ，tf 在 对 Time_Cache 具 体 对 象 进行 操作 和 访问 时 ， 可 以 获取 一 个 坐标 系 和 它 的 父 坐 标 系 在 给 定时 间 范 围 内 任意 时 刻 的 坐标 系 转换 数据 ， 同 时 也 可 以 添加 新 的 转换 数据 
并 且 删 除 已 经 过 时 的 数据 。 以 上 功能 的 实现 都 是 使 得 tf 能 跟踪 任意 时 刻 两 个 坐标 系 转换 的 必要 设计 。 为 了 方便 在 特定 的 时 刻 获取 相应 的 数据 ，Time_Cache 类 中 还 定义 了 直接 获取 最 新 时 刻 的 和 最 早 时 刻 的 转 
换 数 据 的 函数 ， 此 设计 也 在 一 定 程度 上 提升 了 系统 的 效率 。 除 此 之 外 ，Time_Cache 类 中 还 定义 了 三 种 插值 可 能 出 现 异 党 的 情况 ， 它 们 分 别 如 下 。 


“ 只 有 一 个 数据 在 缓存 当中 无 法 进行 插值 。 
需要 进行 播 值 的 数据 对 应 的 时 刻 最 晚 ， 没 有 更 晚 的 数据 可 用 于 播 值 。 
需要 进行 播 值 的 数据 对 应 的 时 刻 最 早 ， 没 有 更 早 的 数据 可 用 于 播 值 。 


通过 对 不 同 插值 异常 情况 的 报告 ， 系 统 可 以 清楚 地 给 出 相应 的 错误 信息 ， 使 用 户 可 以 根据 相应 的 错误 信息 ， 做 出 相应 的 调整 。 总 的 来 说 ， 对 该 类 有 个 好 的 设计 是 整个 tf 系统 提升 与 时 间 相关 的 坐标 系 转 
换 的 计算 效率 的 重要 举措 之 一 。 


(7) 异常 类 一 一 Exception 


在 tf 处 理 数据 的 过 程 中 ， 在 不 同 的 阶段 和 不 同 的 模块 当中 ， 不 可 避免 地 会 出 现 各 种 各 样 的 异常 。 通 过 对 异常 进行 记录 和 处 理 ， 可 以 让 tf 更 好 地 掌控 各 个 模块 的 运行 状态 ， 这 对 维持 整个 系统 会 有 很 重要 的 
作用 。tf 中 定义 的 异常 如 表 4-7 所 示 。 


RAT tf 中 定义 的 异常 


编号 具体 含义 

1 在 结构 树 上 找 不 到 指定 的 坐标 系 

2 在 需要 进行 插值 计算 时 ， 数 据 出 现 异常 
3 基本 数据 结构 中 的 参数 不 满足 条 件 

4 可 阻塞 的 函数 中 出 现 超时 的 情况 


在 碍 找 两 个 坐标 系 之 间 的 转化 关系 时 ， 因 两 个 坐标 系 不 
在 同一 柠 坐 标 系 树 上 的 异 各 


n 


ConnectivityException 





4.3.2 ”tf 关键 模块 的 实现 


1. 与 时 刻 相关 联 的 坐标 系 转换 存储 管理 
(1) 与 时 刻 紧密 相连 的 tf 坐标 系 转换 设计 


Time-Cacha 与 坐标 系 的 转 关 系 如 图 4-15 所 示 。 


std::set<TransformStorage> 










storage — 
B 
fa frame id A 





! Time_C ache 


ch frame id 





图 4-15 Time_Cache 与 坐标 系 转 换 关 系 图 


如 图 4-15 所 示 ， 每 一 个 坐标 系 对 应 一 个 Time_Cache 对 象 ， 图 4-15 中 的 箭头 C 表 明了 这 一 点 ， 稍 头 B 表 示 Time_Cache 有 一 个 成 员 变 量 storage_， 而 粗 箭头 A 表 示 storage 存储 着 多 组 坐标 系 ch_frame id 
和 fa_frame id 之 间 的 转换 ， 并 且 它 们 是 以 时 间 为 序 排列 的 。 通 过 对 storage 的 相关 访问 和 操作 ， 可 以 获取 两 个 坐标 系 在 任意 时 刻 转换 的 数据 ， 并 且 可 以 对 它们 进行 删除 和 修改 。 


(2) 成 员 变 量 storage 访问 


在 对 storage 的 访问 过 程 中 ， 会 遇 到 这 样 一 个 问题 : 需要 知道 在 {时 刻 两 个 坐标 系 之 间 的 转化 数据 。tf 会 以 时 间 为 索引 ， 从 storage_ 中 获取 所 需要 的 数据 。 这 时 可 能 会 出 现 如 下 的 5 种 情形 ， 分 别 如 图 4- 
16、 图 4-17、 图 4-18 图 4-19 和 图 4-20 所 示 (图 中 的 省 略 号 表示 还 有 若干 个 节点 ) 。 
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图 4-16 ”情形 1 


storage 
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图 4-20 ”情形 5 


这 5 种 情形 的 特点 描述 和 处 理 方 式 分 别 如 下 。 


情形 1: 存储 结构 当中 有 在 t 时 刻 前 后 两 个 时 刻 的 两 次 数据 转换 ， 这 种 情况 下 ，Time_Cache 会 调用 函数 interpolate 进 行 插值 计算 ,得 到 在 近似 t 时 刻 的 转换 数据 。 其 中 interpolate 函 数 是 利用 两 个 转化 中 


的 旋转 四 元 式 和 平移 向 量 分 别 进行 球面 插值 和 三 次 插值 得 到 近似 的 数据 。 此 处 ， 前 文 提 到 的 用 四 元 式 表示 旋转 的 必要 性 和 优点 也 得 到 了 体现 。 
情形 2: 存储 结构 中 最 早 的 变换 数据 都 比 给 定 的 时 刻 t 要 晚 ， 会 返回 Time_Cache 类 中 提 到 的 第 三 种 异常 。 
情形 3: 存储 结构 中 最 晚 的 变换 数据 都 比 给 定 的 时 刻 t 要 早 ， 会 返回 Time_Cache 类 中 提 到 的 第 二 种 异常 。 
情形 4: 存储 结构 中 刚好 存在 与 {时刻 对 应 的 转换 数据 ， 直 接 获 取 数 据 即 可 。 
情形 5: 存储 结构 中 只 存在 一 个 时 刻 对 应 的 转换 数据 ， 并 且 不 是 t 时 刻 ， 由 于 只 有 一 个 数据 ， 无 法 进行 插值 ， 因 此 会 返回 Time_Cache 类 中 提 到 的 第 一 种 异常 。 
(3) 成 员 变 量 新 数据 的 插入 


在 storage 中 插入 新 数据 时 ，Time_Cache 会 根据 给 定 插入 转换 数据 所 对 应 的 时 刻 ， 插 入 到 storage 相应 的 位 置 当 中 。 在 插入 的 流程 完成 之 后 ， 还 需要 对 整个 storage 进行 遍历 ， 将 其 中 转换 数据 对 应 
的 时 刻 超过 系统 最 长 存储 时 间 的 存储 数据 予以 删除 ， 以 节省 存储 空间 和 提升 后 续 的 查找 效率 。 


2 .坐标 系 树 形 结构 处 理 
(1) 坐标 系 树 形 结构 的 形成 


前 文 的 论述 中 已 经 提 到 过 ，tf 中 坐标 系 之 间 的 转换 关系 用 Transformstorage 来 存储 ， 在 每 一 个 Transformstorage 对 象 中 都 对 应 着 两 个 坐标 系 ， 一 个 是 转换 中 的 初始 坐标 系 ， 定 义 为 父 坐 标 系 ; 另外 一 
个 是 转换 之 后 得 到 的 坐标 系 ， 定 义 为 子 坐 标 系 。 由 于 tf 限定 了 每 个 坐标 系 只 有 唯一 的 来 源 ， 从 最 初 的 自然 坐标 系 开 始 转换 生成 不 同 的 坐标 ， 这 样 就 形成 了 一 个 树 形 结构 的 坐标 系 关系 图 。 事 实 上 在 内 存 中 ， 
并 没有 真正 存在 一 个 这 样 的 结构 ， 是 通过 分 析 和 整合 各 个 坐标 系 之 间 的 关系 而 得 到 的 一 个 树 形 的 结构 图 ， 其 中 一 个 例子 的 具体 结构 如 图 4-21 所 示 (每 个 大 写字 母 均 表示 一 个 坐标 系 ) 。 


A 








图 4-21 坐标 系 之 间 树 形 结构 图 
(2) 坐标 系 树 遍历 


上 文中 已 经 提 到 过 ， 在 系统 内 存 中 并 不 存在 一 块 存放 坐标 系 树 的 空间 。 事 实 上 ， 在 tf 系统 当中 是 利用 每 个 坐标 系 对 应 的 Time_Cache 对 象 的 成 员 变 量 storage 来 获取 坐标 系 的 父 坐标 系 的 ， 此 时 得 到 的 父 
坐标 系 又 可 以 找到 与 它 对 应 的 父 坐 标 系 ， 以 此 类 推 ， 可 以 实现 从 一 个 节点 到 根 节点 的 访问 。 再 从 另外 一 个 底 端的 坐标 系 开始 访问 ， 也 可 以 访问 到 树 的 根 节 点 ， 以 此 类 推 ， 可 以 完成 整 棵 坐标 系 树 的 遍历 。 


(3) 坐标 系 转换 关系 查找 
假设 有 两 个 坐标 系 X (source frame) 和 Y (target frame) ， 需 要 计算 从 X 到 Y 的 坐标 系 转换 数据 ， 则 X 坐 标 系 和 Y 坐 标 系 在 坐标 系 树 形 结构 中 可 能 会 出 现 以 下 几 种 情形 。 
情形 1: X 和 Y 在 同一 个 坐标 系 ， 它 们 之 间 的 转换 关系 非常 明确 ， 不 需 再 对 树 进行 访问 ， 两 者 之 间 的 转换 关系 就 是 X=Y。 


情形 2: Xf0Y 在 坐标 系 树 的 同一 棵 子 树 上 ， 并 且 X 是 Y 的 父亲 节点 或 祖先 节点 ， 如 图 4-22 或 图 4-23 中 的 A、B 之 间 的 关系 。 





图 4-22 A 和 B 是 父子 关系 





图 4-23 ”A 是 B 的 祖先 节点 


情形 3: X#0Y 在 坐标 系 树 的 同一 棵 子 树 上 ， 并 且 Y 是 X 的 父亲 节点 或 祖先 节点 ， 如 图 4-22 或 图 4-23 中 B、 人 A 之 间 的 关系 。 


情形 4: X 和 Y 不 在 坐标 系 树 的 同一 棵 子 树 上 ， 它 们 有 共同 的 祖先 节点 或 父亲 节点 ， 如 图 4-24 或 图 4-25 中 X 和 Y 的 关系 。 





图 4-24 X 和 Y 有 公共 祖先 








图 4-25 X 和 Y 有 公共 父 节 点 
情形 5: X 和 Y 不 在 同一 棵 坐标 系 树 上 ，tf 会 报告 ConnectivityException 异 常 。 
通过 下 面 的 步骤 可 以 解决 查找 两 个 坐标 系 之 间 转 换 数 据 的 问题 ， 具 体 步 骤 如 下 。 
1) 判断 X 和 Y 是 否 相等 ， 如 果 相 等 则 直接 终止 程序 ， 正 好 对 应 情形 1。 否 则 ， 转 到 步骤 2) 执行 。 


2) 从 Y 坐 标 系 开 始 向 上 依次 访问 其 父亲 节 点 直到 根 节点 为 止 ， 并 进 代 计算 Y 与 当前 访问 坐标 之 间 的 转换 关系 数据 ， 如 果 在 访问 过 程 当中 存在 一 个 坐标 系 为 X， 则 访问 立即 终止 ， 并 且 计 算出 从 Y 到 X 的 坐 
标 系 转换 数据 ， 设 为 Tyx。 如 果 到 根 节点 还 未 能 终止 ， 则 记录 下 这 个 根 节点 和 坐标 系 转换 数据 ， 坐 标 系 转换 数据 设 为 TyR1， 根 节点 设 为 R1， 执 行 步骤 3) 。 


3) 从 X 坐 标 系 开 始 向 上 ， 依 次 访问 其 父亲 节 点 直到 根 节 点 为 止 ， 并 进 代 计算 X 与 当前 访问 坐标 系 之 间 的 转换 关系 数据 ， 如 果 在 访问 过 程 当中 存在 一 个 坐标 系 为 Y， 则 访问 立即 终止 ， 并 且 计 算出 从 X 到 Y 
的 坐标 系 转换 数据 ， 设 为 Txy。 如 果 到 根 节点 还 未 能 终止 ， 则 记录 下 这 个 根 节点 坐标 系 转换 数据 ， 坐 标 系 转换 数据 设 为 TxR2， 根 节点 设 为 R2， 执 行 步 骤 4) 。 


4) 如 果 R1 等 于 R2， 则 说 明 X 和 Y 在 同一 颗 坐 标 系 树 上 ， 此 时 ， 计 算 坐 标 系 变换 T*xy=TxR2* (TyR1 的 逆 ) ， 正 常 终止 。 如 果 不 等 于 则 表示 Xf 和 0Y 不 在 同一 棵 坐标 系 树 上 ， 返 回 异 常 Connectivity- 
Exception, 

通过 上 述 三 个 步骤 的 执行 ， 可 以 看 出 步骤 2) 的 终止 正好 对 应 着 情形 2， 此 时 可 以 得 出 结论 : X 到 Y 的 坐标 系 转化 数据 为 Tyx 的 逆 ; 步骤 3) 的 终止 正好 对 应 情形 3， 此 时 可 以 得 出 结论 : X 到 Y 的 坐标 系 转化 
数据 为 Txy; 步骤 4) 的 正常 终止 正好 对 应 情形 4， 此 时 可 以 得 出 结论 : X 到 Y 的 坐标 系 转化 数据 为 T*xy。 步 又 4) 报 出 的 异常 则 正好 对 应 情形 5。 

3.Transformer 主 要 函数 的 具体 实现 

(1) 添加 新 坐标 系 转换 关系 


在 添加 新 的 坐标 转换 关系 时 ， 其 必要 前 提 是 坐标 转换 关系 中 的 父 坐 标 系 已 经 存在 于 系统 结构 中 。 在 添加 新 的 坐标 系 转换 关系 时 ， 只 需要 利用 转换 关系 中 的 子 坐标 系 的 名 称 ， 在 系统 中 构建 一 个 
Cache_Time 的 对 象 ， 同 时 把 两 个 坐标 系 之 间 的 转换 数据 和 对 应 的 时 刻 以 Transformstorage 的 结构 插入 到 Cache_Time 对 象 的 成 员 变 量 storage_ 当 中 即 可 ， 这 就 完成 了 新 的 坐标 系 转换 的 加 入 过 程 。 当 然 ， 
在 给 子 坐 标 系 构建 Cache_Time 对 象 时 ， 需 要 判断 它 与 父亲 坐标 系 是 否 相 同 ， 为 真 则 报 出 相应 的 错误 。 


(2) 源 坐 标 系 和 目标 坐标 系 之 间 的 转换 数据 查找 


上 文 天 于 Transformer 类 的 介绍 中 已 经 提 到 ， 寻 找 源 坐 标 系 和 目标 坐标 系 之 间 的 转换 数据 ， 分 为 两 种 情形 ， 一 种 是 含有 一 个 固定 不 变 的 中 间 坐 标 系 ， 另 外 一 种 是 直接 计算 两 个 坐标 系 之 间 的 转换 数据 。 
前 一 种 情形 可 以 转换 为 计算 源 坐 标 系 一 中 间 坐 标 系 一 目标 坐标 系 转换 关系 的 数据 相 乘 ， 相 当 于 两 次 第 二 种 情形 的 计算 ， 因 此 ， 这 里 仅 讲述 两 个 坐标 系 之 间 有 直接 转换 关系 的 情形 。 在 已 知 两 个 坐标 系 名 
称 的 前 提 之 下 ， 只 需要 利用 坐标 系 转换 关系 查找 中 的 方法 处 理 即 可 ， 得 出 相应 的 结果 后 ， 一 般 会 以 消息 的 形式 将 得 到 的 结果 传送 给 其 他 模块 使 用 。 








(3) 坐标 转换 天 系 周 期 性 检测 


Transformer 类 中 定义 了 一 个 模块 ， 它 的 功能 是 在 一 定时 间 段 内 每 隔 一 小 段 时 间 就 检测 一 次 两 个 坐标 系 之 间 是 否 存在 转换 关系 ， 该 模块 平时 处 于 阻塞 状态 ， 检 测 到 转换 关系 之 后 ， 模 块 开始 运行 ， 这 样 
做 的 目的 是 为 了 防止 后 面 的 程序 出 现 异常 症状 。 与 此 同时 ， 如 果 在 规定 的 时 间 段 内 均 没有 检测 到 转换 关系 ， 则 会 报 出 相应 的 异常 并 且 终 止 程序 的 继续 执行 。 检 测 转 换 天 系 的 过 程 正 是 不 断 调 用 上 文中 提 到 的 
方法 ， 查 看 是 否 存 在 转换 关系 。 


(4) 源 坐 标 系 和 目标 坐标 系 转换 天 系 判 断 


同样 ， 在 Transformer 类 的 介绍 中 已 经 提 到 过 ， 判 断 源 坐标 系 和 目标 坐标 系 之 间 是 否 存在 转换 天 系 与 寻找 源 坐标 系 和 目标 坐标 系 之 间 的 转化 换 数 据 一 样 ， 都 包含 两 种 不 同 的 类 型 ， 同 理 ， 这 里 只 须 分 别 
判断 源 坐 标 系 和 中 间 坐 标 系 是 否 人 存在 转换 关系 及 中 间 坐 标 系 及 目标 坐标 系 是 否 人 存在 转换 关系 即 可 ， 将 两 者 的 结果 做 与 运算 就 可 以 得 到 最 终 的 结果 。 两 个 坐标 系 之 间 直 接 的 坐标 转换 天 系 只 需 调用 上 文中 步骤 
2) 中 的 方法 ， 如 果 可 以 得 到 一 个 没有 有 异常 的 结果 ， 则 表明 人 存在 这 个 转换 ， 判 断 的 结果 就 为 真 ， 反 之 亦 然 。 


4. 时 间 参 数 及 其 作用 
(1) 时 间 参 数 的 含义 


在 tf 功能 包 的 设计 理念 中 ， 时 间 参 数 指 的 是 具体 的 时 刻 。 为 了 处 理 在 时 间 先 后 关系 上 不 同 坐标 系 之 间 的 关系 ， 萎 定义 了 坐标 系 转换 与 时 刻 之 间 一 一 对 应 的 关系 ， 从 上 文 的 介绍 中 可 以 看 出 ， 每 个 齐 次 变化 
都 对 应 了 一 个 时 间 成 员 变 量 。 时 间 参 数 的 定义 ， 是 tf 能 够 实现 随时 间 变 化 追踪 坐标 系 变换 的 基本 条 件 。 


(2) 时 间 参 数 与 坐标 系 转换 的 关系 


时 间 参 数 和 坐标 系 存在 多 对 多 的 关系 。 同 一 个 时 刻 可 以 对 应 多 个 坐标 系 之 间 的 转换 关系 ， 同 时 ， 两 个 坐标 系 在 不 同 的 时 刻 也 可 以 对 应 多 种 不 同 的 转换 关系 。 在 现实 的 应 用 当中 ，tf 主 要 是 利用 当前 时 刻 
两 个 坐标 系 之 间 的 转换 关系 来 计算 相关 的 数据 。 


(3) 时 间 参 数 在 具体 应 用 实现 中 的 作用 

时 间 参 数 一 般 是 在 定义 一 个 具体 的 齐 次 变化 时 随 之 确定 的 。 它 的 应 用 主要 有 以 下 几 种 情形 。 

* 给 出 具体 时 刻 ， 查 找 此 时 刻 两 个 坐标 系 之 间 的 转换 关系 。 

“ 在 查找 转换 关系 时 ， 若 没有 找到 准确 对 应 时 刻 的 转换 ， 则 需要 利用 时 间 参 数 进行 三 次 插值 和 球面 插值 ， 以 得 到 近似 的 数据 。 同 时 ， 也 可 以 根据 时 间 关 系 给 出 可 能 出 现 的 异常 。 


- 当 给 定 的 时 间 是 ROS 当 前 时 刻 时 ，tf 需 要 调用 专门 的 涵 数 来 处 理 时 间 关 系 。 


44 ”特定 机 器 人 的 ROS tf 应 用 实例 


4.4.1 ”tf 外 部 接口 


1. 新 坐标 系 的 友 布 


tf 功能 包 中 设计 了 TransformBroadcaster 类 用 来 发 布 新 的 坐标 系 。 在 TransformBroadcaster 类 中 最 主要 的 一 个 函数 为 sndTransform (const geometry msgs: : 
Transformstamped&msgtf) 。 其 原理 就 是 调用 setTransform 函 数 ， 在 坐标 系 树 中 添加 新 的 坐标 系 坐 标 转换 ， 同 时 也 实现 了 新 坐标 系 的 发 布 。ROS 中 的 节点 程序 也 可 以 通过 调用 TransformBroadcaster 类 
中 的 函数 ， 以 实现 新 的 坐标 系 的 发 布 。 


2. 转 换 结果 呈现 


tf 功能 包 中 设计 了 TransformListener 类 专门 用 来 计算 参数 在 不 同 坐 标 系 中 的 数据 。 它 的 原理 是 利用 lookupTransform 函 数 找到 两 个 坐标 系 之 间 的 转换 关系 ， 然 后 再 利用 一 个 统一 模式 函数 计算 相关 的 数 
据 ， 该 函数 的 统一 模型 为 : 

















void transformX (const std::string&target frame, const 
geometry msgs: :XStampeq&kstamped in, geometry msgs::XStamped& stamped out) 








其 中 X 表 示 不 同类 型 的 数据 对 象 ， 可 以 是 点 、 向 量 、 旋 转 四 元 式 、 位 姿 等 。target frame 表 示 要 转换 得 到 的 坐标 系 ，stamped in 和 stamped _ out 分别 表示 转换 前 的 数据 和 转换 后 的 数据 。 该 函数 的 原理 
是 通过 查找 两 个 坐标 系 之 间 的 变化 ， 得 到 它们 之 间 的 旋转 关系 和 平移 关系 ， 然 后 利用 矩阵 的 乘法 和 坐标 系 的 加 减 关系 得 到 最 终 的 数据 。ROS 中 的 其 他 节点 程序 可 以 通过 调用 TransformListener 类 中 的 函数 ， 
来 计算 所 需要 的 数据 。 


3. 查 看 两 个 坐标 系 转换 的 参数 


在 tf 功能 包 中 设计 一 个 专门 的 函数 tf_echo， 用 来 输出 两 个 坐标 系 之 间 的 转换 参数 。 举 个 例子 ， 假 设 系统 中 有 X、Y 两 个 坐标 系 ， 它 们 之 间 人 存 在 一 个 转换 关系 ， 在 命令 窗口 中 ， 只 需要 执行 命令 “rosrun tf 
tf_echo X Y”， 就 可 以 得 到 X 和 Y 之 间 转 换 所 包括 的 旋转 四 元 式 和 平移 向 量 的 数据 。 


4. 打 印 与 坐标 系 相关 的 信息 


tf 功能 包 中 设计 了 一 个 专门 的 函数 tf_monitor， 用 来 输出 当前 坐标 系 转换 天 系 树 的 一 些 信息 ， 如 发 布 节点 、 时 间 延 迟 等 。 在 命令 窗口 中 ， 只 需 执行 命令 “rosrun tf tf_monitor”， 就 会 输出 相关 的 信 


on 


44 ”特定 机 器 人 的 ROS tf 应 用 实例 


4.4.1 “tf 间接 口 


1. 新 坐标 系 的 友 布 


tf 功能 包 中 设计 了 TransformBroadcaster 类 用 来 发 布 新 的 坐标 系 。 在 TransformBroadcaster 类 中 最 主要 的 一 个 函数 为 sndTransform (const geometry msgs: : 
TransformStamped&msgtf) 。 其 原理 就 是 调用 setTransform 函 数 ， 在 坐标 系 树 中 添加 新 的 坐标 系 坐 标 转换 ， 同 时 也 实现 了 新 坐标 系 的 发 布 。ROS 中 的 节点 程序 也 可 以 通过 调用 TransformBroadcaster 类 
中 的 函数 ， 以 实现 新 的 坐标 系 的 帮 布 。 


2. 转 换 结果 呈现 


tf 功能 包 中 设计 了 TransformListener 类 专门 用 来 计算 参数 在 不 同 坐 标 系 中 的 数据 。 它 的 原理 是 利用 lookupTransform 函 数 找到 两 个 坐标 系 之 间 的 转换 关系 ， 然 后 再 利用 一 个 统一 模式 函数 计算 相关 的 数 
据 ， 该 函数 的 统一 模型 为 : 

















void transformX (const std::string&target frame, const 
geometry msgs::XStamped&stamped in,geometry msgs::XStamped& stamped out) 








其 中 X 表 示 不 同类 型 的 数据 对 象 ， 可 以 是 点 、 向 量 、 旋 转 四 元 式 、 位 姿 等 。target frame 表 示 要 转换 得 到 的 坐标 系 ，stamped in 和 stamped _ out 分 别 表示 转换 前 的 数据 和 转换 后 的 数据 。 该 函数 的 原理 
是 通过 查找 两 个 坐标 系 之 间 的 变化 ， 得 到 它们 之 间 的 旋转 关系 和 平移 关系 ， 然 后 利用 和 矩阵 的 乘法 和 坐标 系 的 加 减 关系 得 到 最 终 的 数据 。ROS 中 的 其 他 节点 程序 可 以 通过 调用 TransformListener 类 中 的 函数 ， 
来 计算 所 需要 的 数据 。 


3. 查 看 两 个 坐标 系 转换 的 参数 


在 tf 功能 包 中 设计 一 个 专门 的 浮 数 tf_echo， 用 来 输出 两 个 坐标 系 之 间 的 转换 参数 。 举 个 例子 ,假设 系统 中 有 X、Y 两 个 坐标 系 ， 它 们 之 间 存 在 一 个 转换 关系 ， 在 命令 窗口 中 ， 只 需要 执行 命令 “rosrun tf 
tf_echo X Y”， 就 可 以 得 到 X 和 Y 之 间 转 换 所 包括 的 旋转 四 元 式 和 平移 向 量 的 数据 。 


4. 打 印 与 坐标 系 相关 的 信息 


tf 功能 包 中 设计 了 一 个 专门 的 函数 tf_monitor， 用 来 输出 当前 坐标 系 转换 天 系 树 的 一 些 信息 ， 如 发 布 节点 、 时 间 延 迟 等 。 在 命令 窗口 中 ， 只 需 执行 命令 “rosrun tf tf_monitor”， 就 会 输出 相关 的 信 


4.4.2 ROS tf 在 UR5 上 的 应 用 


1.UR5 简 介 


UR5 机 器 人 是 由 丹麦 人 设计 的 一 种 灵活 、 轻 型 和 柔性 的 六 关节 机 器 人 ， 它 的 3D 模 型 图 如 图 4-26 所 示 。 





图 4-26 ”UR5 机 器 人 的 3D 模 型 
UR5 机 器 人 有 6 个 关节 ， 对 应 6 个 自由 度 。 每 个 连 杆 可 对 应 一 个 坐标 系 ， 利 用 这 些 坐 标 之 间 的 关系 ,可 完成 对 UR5 运 动 位 置 和 轨迹 的 计算 。 
2.tf 在 UR5 机 器 人 控制 中 的 作用 
(1) UR5 和 和 ROS 建立 连接 


ROS 并 不 能 直接 在 UR5 机 器 人 平台 上 运行 ， 需 要 利用 运行 于 Linux 系 统 上 的 ROS 去 控制 和 驱动 UR5。 首 先 UR5 通 过 网 线 与 装 有 Linux 系 统 的 电脑 进行 连接 ， 并 且 在 Linux 系 统 上 运行 UR5 机 器 人 的 驱动 程 
序 ， 这 时 Linux 上 的 ROS 就 可 以 与 UR5 通 信 。 在 ROS 上 执行 相关 的 指令 ， 可 以 控制 UR5 机 器 人 的 运动 。 


(2) 坐标 系 天 系 的 展示 


利用 UR5 机 器 人 实体 运动 杆 的 具体 尺寸 和 连接 关系 ， 可 以 在 ROSs 中 建立 UR5 机 器 人 的 坐标 模型 ， 其 中 每 一 个 连 杆 对 应 一 个 坐标 系 。 使 用 可 视 化 工具 rviz 可 以 将 UR5 对 应 的 坐标 系 展 示 出 来 ， 如 图 4-27 所 
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图 4-27 UR5 机 器 人 对 应 的 坐标 系 
(3) tf 功能 包 的 使 用 


如 图 4-27 所 示 ，UR5 的 模型 对 应 了 7 个 不 同 的 坐标 系 ， 它 们 之 间 存 在 转换 关系 ， 并 且 随 着 时 间 的 变化 也 在 不 停 地 变换 。 结 合 UR5 的 模型 ，tf 利 用 发 布 坐标 系 的 方法 在 ROs 中 发 布 了 7 个 坐标 系 ， 它 们 之 间 
的 转换 关系 根据 UR5 当 前 的 形态 和 运动 杆 的 尺寸 来 确定 。 利 用 工具 rviz 就 可 以 得 到 如 图 4-27 所 示 的 关系 图 。 在 UR5 形 态 发 生 改变 的 过 程 中 ， 图 4-27 中 坐标 系 之 间 的 转换 关系 会 发 生 改 变 ， 通 过 不 断 采集 它们 
之 间 的 转换 关系 ， 并 利用 tf 中 的 发 布 转换 关系 国 数 按照 一 定 的 频率 不 断 发 布 它们 之 间 的 转换 关系 到 ROS 当 中 ， 利 用 可 视 化 工具 rviz 不 断 显 示 类 似 于 图 4-27 所 示 的 坐标 系 关 系 图 ， 可 以 在 ROS 中 看 到 UR5 改 变 姿 
态 过 程 中 的 轨迹 。 反 过 来 ， 可 以 通过 不 断 改变 坐标 系 之 间 的 转换 关系 (不 同 坐标 系 的 转换 关系 对 应 到 机 器 人 实体 上 就 是 不 同 的 位 姿 ) 得 到 一 个 已 知 的 运动 轨迹 ， 然 后 利用 控制 运动 相关 的 功能 包 控制 机 器 人 
按照 设计 好 的 运动 轨迹 进行 运动 。 


45 ”本 童 小 结 


本 章 首先 对 空间 坐标 系 描述 和 转换 及 正 向 运动 学 解 算 进 行 了 简单 描述 ， 然 后 对 ROS tf 坐标 转换 包 的 相关 原理 和 实现 进行 了 深入 分 析 ， 最 后 结合 特定 机 器 人 UR5， 对 坐标 转换 的 具体 应 用 作 简 要 介绍 。 


对 于 tf 包 ， 本 章 对 坐标 系 表示 和 转换 涉及 的 数据 结构 及 其 对 应 的 基本 函数 进行 了 详细 介绍 ， 并 对 tf 包 中 的 关键 实现 模块 进行 了 深入 分 析 ， 其 中 包含 了 与 时 刻 相 关 的 坐标 系 转换 存储 管理 ， 坐 标 系 树 结构 的 
建立 、 访 问 、 遍 历 原理 ， 时 间 参 数 ， 从 而 体现 出 时 间 与 坐标 转换 之 间 的 复杂 关系 。 


第 5 草 ROS 任务 调 度 与 有 限 状 态 机 实现 


5.1 ROS 任务 调度 接口 设计 


基于 ROS 平 台 的 应 用 系统 ， 通 过 ROS 的 “服务 ”可 以 实现 任何 请 求 /响应 模型 的 交互 ， 即 客户 端 发 送 任务 请 求 到 服务 器 ， 服 务 器 执行 相应 任务 ， 将 任务 执行 结果 返回 客户 端 。 但 是 某 些 时 人 息 ， 客 户 端 请 求 
的 任务 需要 长 时 间 执 行 ， 客 户 端 希望 能 够 周期 性 的 获取 任务 的 执行 状态 ， 或 者 能 够 在 任务 执行 过 程 时 中 断 任 务 的 执行 ， 因 此 ，ROSs 提 供 了 抢占 式 的 任务 调度 接口 actionlib， 该 接口 不 但 可 以 调度 任务 的 执 
行 ， 而 且 具 备 中 断 任务 、 任 务 状 态 跟踪 与 周期 性 状态 反馈 、 执 行 过 程 中 间 结 果 反 馈 的 能 力 。 


这 里 的 “任务 ” 指 的 就 是 用 户 自 定 义 的 机 器 人 任务 ， 如 智能 机 器 人 自主 识别 及 抓 取 物体 、 机 器 人 依据 规划 路 径 自主 行走 等 ， 具 体 任务 的 程序 由 用 户 来 实现 ，actionlib 接 口 只 是 提供 了 相应 的 调度 接口 。 


在 actionlib 中 ， 客 户 端 发 送 请 求 到 服务 器 端 ， 服 务 器 端 根据 相应 的 任务 调度 策略 进行 任务 调度 ， 被 选择 的 任务 将 会 抢占 当前 任务 的 执行 权 。 同 时 ， 客 户 端 还 可 以 发 送 取消 请 求 ， 服 务 器 端 依照 一 定 的 原 
则 选择 取消 一 个 或 多 个 任务 的 执行 。 


actionlib 接 口中 提供 了 Simple 调 度 策略 ， 具体 策略 为 : 服务 器 端 一 次 只 选择 一 个 任务 进行 调度 执行 ， 后 来 的 任务 总 是 抢占 当前 任务 的 执行 ， 不 管 当前 任务 是 否 执行 完毕 。 


第 ?5 章 ”ROS 任 务 调 大 与 有 限 状 态 机 实现 


5.1 ROS 任务 调度 接口 设计 


基于 ROS 平 台 的 应 用 系统 ， 通 过 ROS 的 “服务 ”可 以 实现 任何 请 求 /响应 模型 的 交互 ， 即 客户 端 发 送 任务 请 求 到 服务 器 ， 服 务 器 执行 相应 任务 ， 将 任务 执行 结果 返回 客户 端 。 但 是 某 些 时 人 息 ， 客 户 端 请 求 
的 任务 需要 长 时 间 执 行 ， 客 户 端 希望 能 够 周期 性 的 获取 任务 的 执行 状态 ， 或 者 能 够 在 任务 执行 过 程 时 中 断 任务 的 执行 ， 因 此 ，ROSs 提 供 了 抢占 式 的 任务 调度 接口 actionlib， 该 接口 不 但 可 以 调度 任务 的 执 
行 ， 而 且 具 备 中 断 任务 、 任 务 状态 跟踪 与 周期 性 状态 反馈 、 执 行 过 程 中 间 结 果 反 馈 的 能 力 。 


这 里 的 “任务 ” 指 的 就 是 用 户 自 定 义 的 机 器 人 任务 ， 如 智能 机 器 人 自主 识别 及 抓 取 物体 、 机 器 人 依据 规划 路 径 自主 行走 等 ， 具 体 任务 的 程序 由 用 户 来 实现 ，actionlib 接 口 只 是 提供 了 相应 的 调度 接口 。 


在 actionlib 中 ， 客 户 端 发 送 请 求 到 服务 器 端 ， 服 务 器 端 根据 相应 的 任务 调度 策略 进行 任务 调度 ， 被 选择 的 任务 将 会 抢占 当前 任务 的 执行 权 。 同 时 ， 客 户 端 还 可 以 发 送 取消 请 求 ， 服 务 器 端 依照 一 定 的 原 
则 选择 取消 一 个 或 多 个 任务 的 执行 。 


actionlib 接 口中 提供 了 Simple 调 度 策略 ， 具体 策略 为 : 服务 器 端 一 次 只 选择 一 个 任务 进行 调度 执行 ， 后 来 的 任务 总 是 抢占 当前 任务 的 执行 ， 不 管 当 前 任务 是 否 执行 完毕 。 


5.1.1 action 的 设计 与 编译 
action 用 于 定义 任务 ， 任 务 定义 包括 目标 (Goal) 、 任 务 执行 过 程 状态 反馈 (Feedback) 和 结果 (Result) 等 。action 是 一 种 与 平台 、 编 程 语言 无 关 的 接口 定义 语言 。 图 5-1 是 一 个 简单 的 action 定 义 
示例 。 


action 的 本 质 是 消息 。action 文 件 依次 定义 了 goal、result、feedback， 定 义 顺 序 不 能 交换 。action 通 过 ROS 中 的 catkin 或 rosbuild 两 种 编译 系统 ， 可 以 生成 多 种 语言 (C++、Python、Java) ， 以 及 
屏蔽 平台 数据 类 型 差异 性 的 类 、 结 构 体 及 消息 。 


编译 action 将 会 自动 产生 7 个 结构 体 ， 分 别 为 Action、ActionGoal、ActionFeedback、ActionResult、Goal、Feedback、Result 结 构 体 。 图 5-2 是 编译 action 生 成 的 消息 。 


goal definition 
intis order 
time endtime 


result definition 
int32[] sequence 


#feedback 
int32L] sequence 





testlboal. msg 
fzoal definition 
intsa order 

time endtime 


testResult. msg 
result definition 
int32L] sequence 


test Feedback. msg 
#ieedback 
int32[] sequence 


5.1.2 ”基于 主题 的 ActionClient 与 ActionServer 的 交互 设计 


actionlib 接 口中 定义 了 ActionClient 与 ActionServer 两 种 角色 ， 分 别 代表 任务 请 求 的 客户 端 和 任务 调度 的 服务 器 端 。 下 面 就 来 介绍 两 种 角色 的 交互 过 程 。 
1. 基 本 框架 


图 5-3 是 ActionClient 与 ActionServer 的 交互 基本 框架 。 


Foal msg | | Foal msg 


cancel msg | | | cancel msg 


Action m | 1% Action 
: result msg | = | 
Client 5 | KERM Bee Server 


feedback msg ， feedback msg 


Status MSF , | Status msg 











图 5-3 ActionClient 5 ActionServer X Z 4E 22 


ActionClient 与 ActionServer 的 交互 采用 了 “发 布 /订阅 ”模型 ， 即 基于 主题 的 异步 数据 流通 信 模 型， 具体 实现 中 依赖 XMLRPC 进 行 远程 过 程 调用 ，XMLRPC 的 底层 实现 采用 的 是 select 异 步 网 络 模型 。 
两 者 的 交互 原理 具体 如 下 。 


1) ActionClient 发 布 Goal 和 Cancel 主 题 ， 并 向 主题 中 发 送 goal 消 息 和 cancel 请 求 消息 。 

2) ActionClient 订 阅 主题 Result、Feedback 及 Status， 并 监听 这 三 个 主题 上 的 消息 。 

3) ActionServer 订 阅 主题 Goal 及 Cancel， 并 监听 这 两 个 主题 上 的 消息 。 

4) ActionServer 发 布 主题 Result、Feedback、status， 并 分 别 向 这 三 个 主题 友 送 result 消 息 、 周 期 性 的 feedback 消 息 及 任务 执行 状态 消息 。 

2. 任 务 区 分 设计 

当 多 个 ActionClient 在 一 定时 间 内 同时 发 送 Goal 消 息 时 ，ActionServer 需 要 区 分 每 一 个 Goal 属 于 哪 一 个 ActionClient， 因 此 必须 保证 每 一 个 Goal 都 有 唯一 的 标识 ，Action-Server 才 能 将 执行 结果 发 送 给 
相应 的 客户 端 。 


actionlib 接 口 设计 了 专用 于 区 分 目标 的 GoallID 类 。GoallD 类 中 有 ID 和 stamp 两 个 成 员 ，1D 是 唯一 标识 每 一 个 Goa| 的 字符 串 ，stamp 代 表 每 一 个 Goal 发 送 的 时 间 戳 。GoallID 类 为 ActionServer 与 
ActionClient 的 交互 提供 了 一 个 稳健 的 方式 ， 来 保证 两 者 之 间 的 任务 请 求 和 执行 结果 的 一 一 对 应 关系 。 


GoallD 的 ID 由 节点 名 、 节 点 计数 及 时 间 戳 构成 。 节 点 名 本 身 具 有 全 局 唯一 性 ;节点 计数 采用 锁 机 制 ， 以 保证 全 局 唯一 ， 再 结合 不 同 的 时 间 戳 ， 来 保证 每 一 个 Goal 均 有 唯一 标识 。 图 5-4 是 简单 的 CoallID 


示例 。 


Goall 


ID:client 100 144765421 
stamp:lé¢47b54e1 


图 5-4 简单 的 GoalID 示 例 





3. 主 题 及 主题 消息 设计 
ActionClient 与 ActionServer 的 交互 涉及 以 下 5 个 主题 。 
- Goal ZAM: 使 用 编译 action 产 生 的 ActionGoal 消 息 ， 该 消息 绑 定 有 Goal 消 息 及 GoalID。 
.Cancel 主 题 : 使 用 GoalID 消 息 ， 客 户 端 根据 GoalID 消 息 取消 相应 的 目标 。 
. Result 主 题 : 使 用 编译 action 产 生 的 ActionResult 类 型 消息 ， 该 主题 用 来 传递 任务 执行 结果 。 
. Feedback 主 题 : 使 用 编译 action 产 生 的 ActionFeedback 消 息 ， 服 务 器 可 以 在 目标 执行 中 周期 性 地 发 送 ActionFeedback 消 息 到 客户 端 ， 客 户 端 根据 ActionFeedback 消 息 跟踪 任务 执行 的 中 间 过 程 。 


` Status 主 题 : 使 用 GoalStatusArtrtay 类 型 消息 ， 服 务 器 将 任务 执行 状态 通过 该 主题 发 送 给 客户 端 ， 客 户 端 完 成 相关 任务 的 状态 更 新 。 


5.1.3 ”ActionClient 与 ActionServer 的 交互 过 程 

ActionClient 与 ActionServer 在 交互 之 前 ， 需 要 事先 定义 action 文 件 ， 通 过 catkin 和 rosbuild 编 译 系统 自动 产生 Goal、Feedback、Result 消 息 及 对 应 的 Goal、Feedback、Result 结 构 体 ， 这 些 消息 及 
结构 体 信息 对 ActionClient 和 ActionServer 均 是 可 见 的 ， 即 ActionClient 和 ActionServer 共 享 这 些 消息 类 的 结构 、 成 员 等 信息 。 

具体 交互 过 程 如 下 。 

1) ActionClient 创 建 Goal 消 息 并 初始 化 ， 将 Goal 消 息 发 送 到 主题 Goal 上 。 

2) 节点 管理 器 ( 即 ROS Master) 通知 ActionServer 接 收 相应 消息 。 

3) ActionServer 将 Goal 加 入 到 任务 列表 ， 经 过 一 定 的 策略 ， 一 次 选择 列表 中 的 一 个 任务 进行 执行 ， 执 行 开 发 者 提供 的 处 理 函 数 。 

A) ActionServer 将 执行 结果 发 布 到 Result 主 题 中 ，ActionClient 接 收 任务 的 执行 结果 。 


ActionServer 在 调度 任务 的 过 程 中 会 更 新 任务 状态 并 周期 性 地 反馈 给 ActionClient， 而 开发 者 提供 的 处 理 函 数 可 以 选择 发 送 中 间 计 算 结 果 给 ActionClient， 需 要 说 明 的 是 ，ActionServer 并 不 会 自己 发 送 
中 间 结 果 ， 因 为 ActionServer 并 不 清楚 开发 者 提供 的 处 理子 数 过 程 。 


5.1.4 action 状 态 变换 

actionlib 接 口 提供 了 任务 状态 追踪 能 力 。ActionServer 与 ActionClient 分 别 维护 一 个 任务 状态 机 。ActionServer 从 接收 任务 到 完成 任务 会 执行 一 系列 的 状态 变换 ， 周 期 性 地 发 送 任 务 状态 到 
ActionClient，ActionClient 依 据 接收 到 的 状态 自主 更 新 自身 维护 的 任务 状态 。Actionlib 接 口中 的 状态 机 主要 以 ActionServer 状 态 机 为 主 ，ActionClient 状 态 机 作为 辅助 使 用 。 

1.ActionServer 任 务 状态 变换 


图 5-5 给 出 了 ActionServer 从 接收 任务 直至 任务 结束 的 一 系列 状态 变化 ， 任 务 结束 不 一 定 是 执行 成 功 ， 可 以 是 成 功 执行 、 拒 绝 执行 、 取 消 执 行 、 抢 占 或 中 断 。ActionServer 端 的 状态 很 大 程度 上 依赖 于 开 
发 者 提供 的 函数 对 任务 的 操作 。 


new goal 










setAccepted setSucced 













pending active ( succeeded ， 





Succed 
rejected 





horted 









setAccepted 









doomed 


setRejected preempting 





recalling © 
- setA borted 


setCanceled setCanceled 





preempted | 








recalled 
Client Triggered Server Triggered 
oo — 


图 5-5 ActionSetvet 任 务 状态 变换 
任务 状态 包括 过 程 状 态 和 终止 状态 ， 过 程 状 态 包括 pending、active、recalling、preempting， 终 止 状态 包括 rejected、recalled、preempted、aborted 及 succeeded， 各 状态 的 含义 具体 如 下 。 


- pending 状 态 代 表 任 务 正 在 等 待 ActionSetvet 执 行 。 


` active 状 态 表示 任务 正在 执行 。 

recalling 状 态 表示 任务 在 等 待 执行 期 间 收 到 ActionClient 的 任务 取消 请 求 ， 但 ActionServer 尚 未 确认 该 任务 取消 。 

- preempting 状 态 表 示 任 务 在 执行 过 程 中 接收 到 ActionClient 的 任务 取消 请 求 ， 但 ActionServer 未 确认 该 任务 取消 。 

` rejected 状 态 是 指 任 务 未 开始 执行 且 ActionServer 在 没有 接受 到 ActionClient 的 取消 请 求 的 情况 下 拒绝 执行 任务 。 

recalled 状 态 表 示 任 务 未 开始 执行 时 ActionServer 接 收 到 任务 取消 请 求 ， 或 者 被 新 的 任务 抢占 执行 。 

- abotted 状 态 表示 任务 在 ActionServet 未 接收 到 任务 取消 请 求情 况 下 被 放弃 执行 。 

` preempted 状 态 表 示 任 务 执行 过 程 中 被 新 任务 抢占 ， 或 者 ActionServer 接 收 到 任务 取消 请 求 。 

.succeeded 状 态 表示 任务 成 功 执行 。 

2.ActionClient 任 务 状 态 变 换 

图 5-6 显 示 了 ActionClient 根 据 Actionserver 的 状态 反馈 执行 的 任务 状态 转换 ， 任 务 完成 状态 可 以 是 成 功 执行 、 被 拒绝 执行 、 被 放弃 执行 、 被 抢占 ， 即 一 定 要 达到 终止 状态 。 


ActionClient 跟 踪 的 任务 状态 转换 是 根据 ActionServer 的 周期 性 反馈 的 状态 进行 状态 判断 ， 然 后 执行 相应 的 状态 转换 ， 但 是 由 于 网 络 通信 不 可 靠 等 原因 ，ActionClient 不 一 定 能 够 全 部 接收 到 这 些 周期 性 
状态 消息 ， 因 此 ActionClient 的 任务 状态 不 能 完全 按照 图 5-6 中 显示 的 状态 转换 图 进行 转换 ， 因 此 ActionClient 具 备 对 任务 状态 的 跟踪 可 以 跳 过 未 接收 的 过 时 状态 的 能 力 。 


ActionClient 跟 踪 的 任务 状态 包括 瞬时 状态 和 终止 状态 。 瞬 时 状态 包括 wait for goal ack、pending、active、recalling、preempting、wait for cancel ack、wait for result 等 状态 ， 终 止 状 态 为 
done， 各 状态 的 含义 具体 如 下 。 


wait_fof gpal_ack 表 示 ActionClient 在 发 送 任务 请 求 后 还 未 接收 到 服务 器 端 发 布 的 状态 更 新 前 的 状态 。 
bending 表示 任务 处 于 等 待 执行 的 状态 。 


:active 表示 任务 正在 执行 。 


send goal 












wait for goal ack 





server: pending server:active 


cancel goal 





cancel 


cancel goal 






server:active 





wait for cancel ack 









iiia server: server: ee 
Pe i 1 | Y . | . . 
J . | preempting 
recalling preempting Y ` 
recalling -| preempting 









server: 


server: 
aborted 










recalled ps server: 
rekected a 4 J aborted 
succeede rejected 
succeeded 







wait for result 


receive | result 





Server Triggered Client Triggered 
一 —— l 
图 5-6 ”ActionClient 任 务 状态 变换 
recalling 表 示 任 务 在 等 待 执 行 时 ，ActionServer 接 收 到 取消 请 求 但 未 确认 取消 任务 。 
- preempting 表 示 任 务 在 执行 过 程 ，ActionServer 接 收 到 取消 请 求 但 未 确认 取消 任务 。 
- wait_for_cancel_ack 表 示 任 务 处 于 等 待 ActionServer 确 认 取 消 任务 的 状态 。 
wait_for_result 表 示 等 待 任务 执行 结果 。 


done 表示 任 务 完成 ， 可 细 分 为 成 功 执 行 、 被 拒绝 执行 、 被 放弃 执行 或 被 抢占 4 种 情况 。 


goal 





从 上 述 服 务 器 与 客户 端 任务 状态 的 定义 中 可 以 看 出 ， 相 比 一 般 实时 操作 系统 的 任务 状态 及 其 迁移 ，ROS 的 任务 状态 更 为 细 化， 迁移 过 程 也 更 为 复杂 ， 其 设计 主要 是 为 了 适应 智能 机 器 人 操作 并 发 、 通 信 
故障 及 物理 过 程 影响 等 特性 。 


5.1.5 actionlib 的 任务 调度 策略 


actionlib 接 口 是 一 个 抢占 式 的 任务 调度 接口 ，ActionServer 维 护 着 一 个 任务 队列 ， 所 有 需要 执行 的 任务 都 在 队列 中 ，ActionServer 按 照 Simple 策 略 从 队列 中 选取 任务 进行 调度 。 
1.Simple 策 略 

actionlib 接 口中 集成 Simple 调 度 策略 ， 并 且 实 现 了 SimpleActionServer 及 SimpleAction-Client。Simple 策 略 的 抢占 原则 具体 如 下 。 

1) ActionServer 端 可 能 会 在 很 短 的 时 间 内 同时 接收 到 来 自 多 个 ActionClient 的 任务 请 求 ，Simple 策 略 规定 一 次 仅 有 一 个 任务 处 于 active 状 态 ， 即 一 次 只 有 一 个 任务 被 选择 执行 。 
2) Simple 策 略 根据 任务 到 达 时 间 的 先后 来 判断 任务 优先 级 ， 每 当 SimpleAction-Server 接 收 到 任务 时 ， 后 来 的 任务 总 是 抢占 当前 的 任务 ， 并 变 成 新 的 当前 任务 。 

3) 被 抢占 后 的 任务 不 管 是 否 执行 完毕 ， 都 将 不 会 得 到 9%impleActionServer 的 重新 调度 。 

2. 任 务 取消 策略 


actionlib 接 口中 ActionClient 通 过 发 送 GoallD 消 息 取消 任务 ， 具 体 策略 如 图 5-7 所 示 。 


ID 


取消 所 取消 ID 匹配 
有 任务 的 任务 


的 任务 “| 及 ID 匹配 的 任务 





apes 3 
by a }e% (stamp ) 
图 5-7 ”任务 取消 策略 
. 设置 GoalID 消 息 中 的 ID 为 空 字 符 串 、 时 间 惟 stamp 为 0， 则 ActionServer 取 消 所 有 任务 。 
- 设置 GoalID 消 息 的 ID 为 ActionClieht 所 追踪 的 当前 任务 的 ID ， 则 ActionSetrvet 取 消 该 ID 标识 的 任务 。 
- 设置 GoalID 消 息 的 ID 为 空 字符 串 、 时 间 稚 stamp 为 菜 一 时 间 ， 则 ActionServer 取 消 任 务 时 间 惟 在 该 时 间 惟 之 前 的 所 有 任务 。 
- 设置 GoalID 消 息 的 ID 为 ActionClient 所 追踪 的 当前 任务 LID、 时 间 玲 stamp 为 某 一 指定 时 间 ， 则 服务 器 端 取消 该 ID 标识 且 任 务 时间 崔 在 Stamp 指 定时 间 之 前 的 所 有 任务 。 


3.Simple 策 略 下 任务 状态 的 简化 


在 Simple 策 略 下 ，ActionClient 对 所 追踪 的 任务 状态 变换 进行 了 简化 设计 。 简 化 后 的 状态 变换 如 图 5-8 所 示 。 


pending: 
walt_for_goal_ack, 
pending, recalling 
Done: 


active: 
active, preempting, 
Walt _for cancel ack 


Walt for result, 


done 





图 5-8 简化 后 客户 端的 任务 状态 


Simple 策略 后 简化 的 客户 端的 任务 状态 有 pending、active 和 done。 


5.1.6 ”actionlib 接 口 的 具体 实现 


actionlib 的 服务 器 端 和 客户 端 类 图 设计 如 图 5-9 和 图 5-10 所 示 。 


DestructionGuard 


+tryProtect QO :bool 









a = 
ActionServerBase ServerGoalHandle 
+goal:boost::shared_p| x nj #status_list:list -goal:boost::shared_p 
tr<ActionGoal> <StatusTracker> tr<ActionGoal> 
+goalCallback -setSucceeded 
(Goal) :void i (Result, string):void 
b 
家 


* 


SimpleåctionServer 


—-current_goal: 
GoalHandle 


—setSucceeded 









ActionServer 


-goal_sub:Subscriber 
-initialize :void 


1 
HandleTrackerDeleter 


-zgoal:boost::shared_p 
tr<ActionGoal> 


—setSucceeded 
(Result, string) :void 


(Result, string) :void 


A5-9 ”服务 器 端 类 图 


Foal [DGenerator DestructionGuard 


+name: string -use_count:int 


+generaetID() :GoalID +tryProtect () :bool 











() + 
1 
-list:ManagedList > — -goal Subscribers :Map 
+initGoal aa +waitForStart 
(Goal) :GoalHandleT (Duration) :bool 
| 





plapleActionllient 


ClientGoalHandle 


-en:GoalNanager —-ac:Actionllient 


+cancel () :void +getResult () :Result 





slmpleClientState 


CommState 


+state:stateFnum 


+state:StateEnum +state:StateEnum 





+toString © :string +toString () :string +toString () :string 


图 5-10 客户 端 类 图 


在 服务 器 端 ，ActionServer 类 继承 ActionServerBase， 该 类 用 于 任务 实体 存储 ， 任 务 到 达 后 创建 StatusTracker 类 对 象 用 于 保存 该 任务 ， 同 时 创建 ServerGoalHandle 对 象 用 于 管理 任务 。ActionServer 
类 对 任务 进行 调度 ， 周 期 性 发 布 任务 执行 状态 。SimpleAction-Server 类 是 Simple 策 略 的 具体 实现 者 ， 可 调用 ActionServer 对 象 进行 任务 管理 。 


在 客户 端 ，ActionClient 类 创建 GoalManager 对 象 和 ConnectionMonitor 对 象 ， 前 者 通过 创建 GoallDGenerator 对 象 产 生 GoallD， 后 者 则 实现 服务 器 端 连接 。ActionClient 通 过 一 系列 的 对 象 创建 ， 
可 实现 任务 状态 追踪 和 功能 子 对 象 的 建立 。 


1. 公 共 类 设计 
公共 类 的 类 名 及 其 作用 如 表 5-1 所 示 。 


表 5-1 公共 类 


类 名 作用 

该 类 用 于 定义 目 动 生成 的 goal, feedback, result 消息 ， 有 具体 包括 ActionGoal Goal, 
ActionResult, Result, ActionFeedback, Feedback 

该 类 用 于 生成 goal 的 ID。 该 类 采用 boost 锁 机 制 产生 全 局 唯一 ID，ID 的 格式 为 : 


action definition 


GoalIDGenerator 
Oa SEIS’ NAGE goal FRE RE TL 
AA F Bi IE FESE RT Se RI, ERAT RAY SAITO. AAR A BAS | 
DestructionGuard 
用 计数 实现 共享 对 象 及 对 其 的 回收 
该 类 是 一 个 删除 硕 类 ， 用 于 boost 共享 指针 的 创建 ， 在 共享 指针 被 回收 时 目 动 调用 
EnclosureDeleter 
该 类 做 一 些 资 源 的 释放 工作 
— TT 该 类 是 一 个 STL 容 需 类 ， 使 用 引用 计数 是 实现 对 象 共 享 ， 并 且 提 供 对 容 需 元 素 的 管 
1 i 7 ee no - 一 大 一 
理 ， 当 容器 中 的 对 象 不 在 生命 周期 范围 内 时 ， 自 动 回收 对 象 
| 该 类 用 于 封装 客户 端 状 态 枚 举 类 型 ， 表 示 客 户 端 追 踪 的 任务 状态 。 具 体 任 务 状态 包 
ClientGoalStatus | 
ffi: pending, active, preempted, aborted, succeeded, rejected, lost 
2. 服 务 器 端 类 设计 


服务 器 端 类 的 类 名 及 其 作用 如 表 5-2 所 示 。 


R52 ”服务 器 端 类 


类 名 作用 
StatueTracker 该 类 帮助 ActionServer 存储 每 一 个 任务 的 状态 。 有 具体 存储 着 ActionGoal 的 引用 、 任 
务 状态 (GoalStatus )、 任 务 删 除 时 间 
HandleTracker 该 类 是 一 个 删除 大 类 ， 具 体 存 储 关 服务 如 并 任务 在 列表 中 的 索引 ， 当 任务 的 最 后 一 
Deleter 个 管理 天 对 象 引 用 被 删 除 时 将 任务 从 服务 硕 列 表 中 移 除 
该 类 是 服务 需 端 任务 管理 大 ， 生 ee MES (Action) 的 引用 、 任 务 索引 、 
服务 硕 对 象 引 用 。 主 要 功能 包括 受 任务 ( setAccepted)、 取 消 任 务 (setRejected ), 
ServerGoalHandle 


放弃 执行 任务 (setAborted)、 设 置 任 hs -成 功 (setSucceeded)、 发 布 反馈 (publishFeedback)、 
获取 任务 、 获 取 任 务 ID 及 seu 务 状态 等 
该 类 是 服务 右 病 的 基础 抽象 类 ， 主 要 实现 ActionServer 的 基本 逻辑 ， 包 括 订 阅 主 题 ， 


ActionServerBase 发 布 结果 、 反 馈 或 状态 ， 监 听 来 自 客户 端的 任务 及 取消 请 求 等 
该 类 继承 自 ActionServerBase %, 管理 来 自 客户 端的 任务 及 取消 请 求 ， 允 许 开发 者 
ActionServer ee AAA FEL PIRRU PE AGNIR, RESIS, ER 
发 布 结果 、 过 程 反 馈 及 任务 状态 
SimpleAction 该 类 是 采用 Simple 策略 的 ActionServer 具体 实现 ， 通 过 ActionServer 类 实现 任务 和 取消 
Server TA A H a] eK ACSI Simple 策略 调度 ， 人 然后 使 用 用 户 目 定义 回调 函数 处 理 任 务 


3. 客 户 端 类 设计 
客户 端 类 的 类 名 及 其 作用 如 表 5-3 所 示 。 
表 5-3 客户 端 类 
类 名 作用 
该 类 封装 客户 问 当 前 任务 的 状态 。 具 体 状 态 包括 : wait for goal ack, pending, active, 


CommState , . “or ic gi 
recalling, preempting, wait for cancel ack, wait for result 
该 类 封装 任务 完成 的 终止 状态 。 有 具体 状态 包括 : recalled. preempted succeeded. rejected, 
TerminalState 
aborted, lost 
该 类 用 于 管理 任务 ， 包 括 初 始 化 GoalID 、 存 储 任务 状态 ， 发 布 任务 状态 、 结 末 及 过 
GoalManager 


fe 2 iA E 


类 名 作用 
沪 类 用 于 封闭 和 管理 一 个 任务 的 处 理 过 程 ， 该 类 采用 引用 计数 的 方法 实现 该 类 对 象 的 


A 

ClientGoalHandle 

共享 与 析 构 
CommStateMachine | 该 类 用 于 追踪 服务 右 病 发布 的 任务 状态 ， 该 类 依据 服务 各 并 发 布 的 状态 处 理 状 态 
Connection 、 Ben a ee. ea eR ees ere 

| AFR AE PR FS ein, MES RAMPO ep oR IE Fe 
Monitor 
. . 该 类 表示 客户 病 类 ， 用 于 发 送 任务 请 求 ， 追 踊 任 务 的 状态 ， 获 取 任 务 的 过 程 反 馈 ， 获 

ActionClient piconet eta 

取 任 务 的 执行 结 来 
SimpleGoalState 该 类 是 Simple 束 上 略 人 简化 的 客户 病 任 务 状态 类 ， 简 化 后 的 状态 包括 : pending, active, done 
SimpleClient 该 类 封装 服务 磊 羡 反馈 给 客户 闯 的 任务 状态 ， 具体 状态 包括 : pending 、active recalled, 
GoalState rejected, preempted, aborted, succeeded, lost 


5.2 ”ROS 有 限 状 态 机 的 SMACH 


5.2.1 有限 状态 机 的 基本 原理 


1. 有 限 状态 机 的 定义 

有 限 状 态 机 是 由 有 限 的 状态 及 其 相互 之 间 的 转换 构成 。 有 限 状态 机 在 任何 时 候 都 只 能 处 于 某 一 给 定 的 状态 。 当 接收 到 输入 事件 时 ， 状 态 机 产生 相应 的 输出 ， 同 时 也 可 能 伴随 着 状态 的 转换 。 
有 限 状态 机 M 可 以 形式 化 表示 为 一 个 五 元 组 : M= (Q, >，65，q0,，F) ， 其 中 各 项 含义 如 下 。 

` Q 是 一 个 有 限 的 状态 集合 。 

. 卫 表 示 系 统 能 接收 的 所 有 事件 的 集合 。 

. 8 是 状态 转移 函数 。 

. qd0EQ 是 系统 的 特殊 状态 ， 一 般 是 系统 启动 时 的 初始 状态 。 

.FEQ 是 终止 状态 的 集合 。 

有 限 状态 机 描述 了 某 个 特定 对 象 的 所 有 状态 及 其 转换 的 过 程 ， 对 象 的 状态 转换 由 事件 驱动 ， 对 象 通过 对 事件 接收 的 响应 来 改变 当前 状态 ， 状 态 转 换 图 是 有 限 状 态 机 的 确切 描述 方法 。 
2. 有 限 状 态 机 的 要 素 

有 限 状 态 机 包含 4 个 要 素 ， 具 体 如 下 。 


态 : 指 状态 机 当前 所 处 的 状态 。 


Š 


-RA IRRA FIE 。 当 某 一 条 件 满 足 时 ， 将 触发 相应 的 事件 ， 或 者 执行 相应 状态 的 迁移 。 

: 动作: 条 件 满足 后 执行 的 动作 。 动 作 执 行 ， 可 以 迁移 到 新 的 状态 ， 或 者 仍旧 保持 原状 态 。 条 件 满足 时 ， 也 可 以 不 执行 任何 动作 ， 直 接 迁 移 到 新 状态 。 
ARS: 条 件 满足 后 要 转换 到 的 状态 。“ 次 态 ” 是 相对 于 “ 现 态 ” 而 言 的 ，“ 次 态 ” 一 旦 被 激活 ， 就 会 成 为 新 的 “ 现 态 

如 果 我 们 做 进一步 的 归纳 ， 把 “ 现 态 ” 和 “次 态 ” 统 一 起 来 ， 而 忽略 “动作 ”的 话 ， 那 么 状态 机 就 有 两 个 基本 且 关 键 的 要 素 : 状态 和 迁移 条 件 。 

3. 有 限 状 态 机 的 状态 迁移 


动作 迁移 图 是 一 种 描述 系统 状态 、 状 态 转 换 关系 的 形象 化 图 形 表示 方式 ， 如 图 5-11 所 示 。 





图 5-11 有 限 状 态 机 动作 迁移 图 


4. 有 限 状 态 机 的 应 用 条 件 


. 被 模型 化 的 系统 且 具 有 有 限 个 状态 。 

. 系统 在 一 定 状态 下 的 行为 响应 是 唯一 的 。 

. 系统 在 任何 时 间 段 内 总 会 停留 在 某 一 状态 下 。 
. 系统 状态 迁移 的 条 件 是 有 限 的 。 

. 迁移 是 系统 对 事件 的 反应 。 

. 迁移 所 用 的 时 间 近 似 为 零 。 

5. 智 能 机 器 人 的 有 限 状 态 机 应 用 


机 器 人 是 以 微 控制 器 为 控制 核心 的 ， 微 控制 器 内 谋 程 序 的 运行 决定 了 机 器 人 的 动作 或 行为 。 如 何 为 不 同 应 用 的 机 器 人 设计 控制 程序 一 直 是 困扰 着 设计 人 员 的 难题 之 一 。 在 明确 需求 的 基础 上 ， 将 工作 任 
务 细 分 ， 确 定 各 个 任务 之 间 的 内 在 联系 ， 就 可 以 用 有 限 状 态 机 模型 来 指导 不 同类 型 机 器 人 控制 程序 的 设计 。 有 限 状 态 机 是 一 种 记录 给 定时 刻 状 态 的 数学 模型 ， 根 据 输入 而 改变 其 状态 或 引发 一 个 动作 。 应 用 
有 限 状 态 机 或 状态 转换 方法 实现 机 器 人 控制 程序 设计 ， 可 以 理 清 控制 思路 ， 简 化 设计 过 程 。 


ROS 作 为 面向 机 器 人 的 操作 系统 ， 控 制程 序 设 计 支 持 尤为 重要 。 为 了 提高 代码 复 用 ， 适 应 不 同 应 用 场景 的 机 器 人 控制 要 求 ， 有 限 状 态 机 SMACH 应 运 而 生 。SMACH 提 供 了 机 器 人 任务 控制 所 需 的 不 同 状 
态 和 状态 机 容器 ， 使 得 设计 人 员 在 面 对 特定 的 机 器 人 控制 程序 设计 时 ， 能 够 组 合 不 同类 型 的 状态 机 ， 不 但 可 提高 代码 的 复 用 性 ， 同 时 设计 也 更 加 简单 高 效 。 


这 里 需要 说 明 的 是 ， 有 限 状态 机 和 SMACH 中 的 状态 机 既 有 联系 又 有 区 别 。 在 通用 有 限 状 态 机 模型 中 ， 事 件 是 状态 转移 的 触发 因素 ， 某 一 事件 的 出 现 使 得 相应 的 动作 被 执行 ， 或 者 发 生 状 态 转 移 ， 状 态 
机 从 当前 状态 转移 到 下 一 个 状态 。 状 态 反映 的 是 系统 从 开始 到 现在 时 刻 的 输入 变化 。 转 移 是 两 个 状态 之 间 的 关系 ， 它 由 某 个 事件 触发 ， 然 后 执行 特定 的 操作 或 评估 并 导致 特定 的 结束 状态 。 动 作 是 在 给 定时 
刻 要 进行 的 活动 描述 ， 是 系统 此 刻 需 要 执行 的 活动 。 而 SMACH 设 计 的 初衷 是 用 于 描述 具有 层次 结构 的 机 器 人 高 层 复杂 任务 。 机 器 人 的 复杂 任务 可 以 被 逐 层 分 解 成 可 执行 的 子 任务 ， 然 后 用 SMACH 的 状态 明 
确定 义 这 些 可 执行 的 子 任务 ， 状 态 之 间 通 过 状态 转移 条 件 关 联 ， 这 样 就 可 以 用 状态 机 的 形式 明确 表示 一 个 复杂 的 机 器 人 任务 。 因 此 可 以 看 出 ，SMACH 中 的 状态 是 对 机 器 人 任务 执行 过 程 中 某 一 子 任务 的 摘 
述 ， 而 状态 转移 则 表示 了 当前 状态 到 下 一 状态 的 对 应 关系 及 转移 条 件 ， 因 此 当前 状态 执行 结束 的 情形 决定 了 状态 转移 的 方向 。 


5.2 ” ROS 有限 状态 机 的 SMACH 


5.2.1 有限 状 态 机 的 基本 原理 


1. 有 限 状态 机 的 定义 
有 限 状 态 机 是 由 有 限 的 状态 及 其 相互 之 间 的 转换 构成 。 有 限 状态 机 在 任何 时 候 都 只 能 处 于 某 一 给 定 的 状态 。 当 接收 到 输入 事件 时 ， 状 态 机 产生 相应 的 输出 ， 同 时 也 可 能 伴随 着 状态 的 转换 。 
有 限 状态 机 M 可 以 形式 化 表示 为 一 个 五 元 组 : M= (Q, x, 6, q0, F) ， 其 中 各 项 含义 如 下 。 

`Q 是 一 个 有 限 的 状态 集合 。 


. 卫 表 示 系 统 能 接收 的 所 有 事件 的 集合 。 


SERRE BK, 

. qd0EQ 是 系统 的 特殊 状态 ， 一 般 是 系统 启动 时 的 初始 状态 。 

. FEQ 是 终止 状态 的 集合 。 

有 限 状 态 机 描述 了 某 个 特定 对 象 的 所 有 状态 及 其 转换 的 过 程 ， 对 象 的 状态 转换 由 事件 驱动 ， 对 象 通 过 对 事件 接收 的 响应 来 改变 当前 状态 ， 状 态 转 换 图 是 有 限 状 态 机 的 确切 描述 方法 。 
2. 有 限 状 态 机 的 要 素 

有 限 状态 机 包含 4 个 要 素 ， 具体 如 下 。 

RA: 指 状态 机 当前 所 处 的 状态 。 

. 条 件 : 又 被 称 为 “事件 ”。 当 某 一 条 件 满足 时 ， 将 触发 相应 的 事件 ， 或 者 执行 相应 状态 的 迁移 。 

. 动作 : 条 件 满足 后 执行 的 动作 。 动 作 执行 完毕 后 ， 可 以 迁移 到 新 的 状态 ， 或 者 仍旧 保持 原状 态 。 条 件 满足 时 ， 也 可 以 不 执行 任何 动作 ， 直 接 迁 移 到 新 状态 。 
KA: 条 件 满足 后 要 转换 到 的 状态 。“ 次 态 ” 是 相对 于 “ 现 态 ”而 言 的 ，“ 次 态 ”一 旦 被 激活 ， 就 会 成 为 新 的 “ 现 态 ” 

如 果 我 们 做 进一步 的 归纳 ， 把 “ 现 态 ”和 “次 态 ” 统 一 起 来 ， 而 忽略 “动作 ” 的话， 那么 状态 机 就 有 两 个 基本 且 关 键 的 要 素 : 状态 和 迁移 条 件 。 

3. 有 限 状 态 机 的 状态 迁移 


动作 迁移 图 是 一 种 描述 系统 状态 、 状 态 转换 关系 的 形象 化 图 形 表示 方式 ， 如 图 5-11 所 示 。 
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图 5-11 有 限 状态 机 动作 迁移 图 
4. 有 限 状 态 机 的 应 用 条 件 
有 限 状 态 机 的 优点 在 于 简单 易 用 ， 状 态 间 的 关系 简明 直观 。 一 般 情 况 下 ， 可 以 应 用 有 限 状态 机 的 前 提包 括 如 下 几 点 。 
. 被 模型 化 的 系统 且 具 有 有 限 个 状态 。 
- 系统 在 一 定 状 态 下 的 行为 响应 是 唯一 的 。 
- 系统 在 任何 时 间 段 内 总 会 停留 在 某 一 状态 下 。 
- 系统 状态 迁移 的 条 件 是 有 限 的 。 
- 迁移 是 系统 对 事件 的 反应 。 
- 迁移 所 用 的 时 间 近 似 为 堆 。 
5. 智 能 机 器 人 的 有 限 状态 机 应 用 


机 器 人 是 以 微 控制 器 为 控制 核心 的 ， 微 控制 器 内 谋 程 序 的 运行 决定 了 机 器 人 的 动作 或 行为 。 如 何 为 不 同 应 用 的 机 器 人 设计 控制 程序 一 直 是 困扰 着 设计 人 员 的 难题 之 一 。 在 明确 需求 的 基础 上 ， 将 工作 任 
务 细 分 ， 确 定 各 个 任务 之 间 的 内 在 联系 ， 就 可 以 用 有 限 状 态 机 模型 来 指导 不 同类 型 机 器 人 控制 程序 的 设计 。 有 限 状 态 机 是 一 种 记录 给 定时 刻 状 态 的 数学 模型 ， 根 据 输入 而 改变 其 状态 或 引发 一 个 动作 。 应 用 
有 限 状 态 机 或 状态 转换 方法 实现 机 器 人 控制 程序 设计 ， 可 以 理 清 控制 思路 ， 简 化 设计 过 程 。 


ROS 作 为 面向 机 器 人 的 操作 系统 ， 控 制程 序 设 计 支 持 尤 为 重要 。 为 了 提高 代码 复 用 ， 适 应 不 同 应 用 场景 的 机 器 人 控制 要 求 ， 有 限 状 态 机 SMACH 应 运 而 生 。SMACH 提 供 了 机 器 人 任务 控制 所 需 的 不 同 状 
态 和 状态 机 容器 ， 使 得 设计 人 员 在 面 对 特 定 的 机 器 人 控制 程序 设计 时 ， 能 够 组 合 不 同类 型 的 状态 机 ， 不 但 可 提高 代码 的 复 用 性 ， 同 时 设计 也 更 加 简单 高 效 。 


这 里 需要 说 明 的 是 ， 有 限 状态 机 和 SMACH 中 的 状态 机 既 有 联系 又 有 区 别 。 在 通用 有 限 状 态 机 模型 中 ， 事 件 是 状态 转移 的 触发 因素 ， 某 一 事件 的 出 现 使 得 相应 的 动作 被 执行 ， 或 者 发 生 状 态 转 移 ， 状 态 
机 从 当前 状态 转移 到 下 一 个 状态 。 状 态 反映 的 是 系统 从 开始 到 现在 时 刻 的 输入 变化 。 转 移 是 两 个 状态 之 间 的 关系 ， 它 由 某 个 事件 触发 ， 然 后 执行 特定 的 操作 或 评估 并 导致 特定 的 结束 状态 。 动 作 是 在 给 定时 
刻 要 进行 的 活动 描述 ， 是 系统 此 刻 需 要 执行 的 活动 。 而 SMACH 设 计 的 初衷 是 用 于 描述 具有 层次 结构 的 机 器 人 高 层 复杂 任务 。 机 器 人 的 复杂 任务 可 以 被 逐 层 分 解 成 可 执行 的 子 任务 ， 然 后 用 SMACH 的 状态 明 
确定 义 这 些 可 执行 的 子 任务 ， 状 态 之 间 通 过 状态 转移 条 件 关联 ， 这 样 就 可 以 用 状态 机 的 形式 明确 表示 一 个 复杂 的 机 器 人 任务 。 因 此 可 以 看 出 ，SMACH 中 的 状态 是 对 机 器 人 任务 执行 过 程 中 某 一 子 任务 的 摘 
述 ， 而 状态 转移 则 表示 了 当前 状态 到 下 一 状态 的 对 应 关系 及 转移 条 件 ， 因 此 当前 状态 执行 结束 的 情形 决定 了 状态 转移 的 方向 。 


5.2.2 SMACH 概 述 


1.SMACH 人 简介 


SMACH 是 StateMachine 的 缩写 。SMACH 是 利用 独立 于 ROS 的 Python 库 构建 的 层次 状态 机 库 ， 用 于 快速 创建 健壮 、 可 维护 、 模 块 化 的 机 器 人 行为 代码 。 它 具有 任务 级 执行 和 协调 的 功能 ， 并 且 提 供 了 
多 种 状态 和 状态 机 容器 ， 而 容器 又 是 特殊 的 状态 ， 因 此 可 以 作为 其 他 容器 中 的 状态 。 由 于 某 些 复杂 的 结构 化 任务 可 以 明确 定义 而 且 能 够 利用 状态 机 清晰 地 描述 ， 所 以 SMACH 可 以 用 于 实现 任务 级 的 机 器 人 
控制 ， 而 无 结构 化 的 任务 可 以 在 不 同系 统 的 更 高 层 处 理 。 


由 于 Python 语言 对 于 智能 机 器 人 编程 具有 优势 ， 因 此 ROSs 采 用 Python 应 用 编程 接口 开发 展 次 任务 的 状态 机 。SMACH 作 为 ROs 的 核心 之 一 ， 不 仅 能 够 用 来 建立 层次 化 的 状态 机 ， 而 且 可 以 建立 任何 其 他 
遵守 接口 约定 的 任务 状态 容器 。 尽 管 SMACH 是 独立 于 ROS 的 库 ， 但 是 相当 多 的 内 容 已 经 写 入 到 smach_ros 包 中 ， 用 于 实现 与 ROS 系 统 通 信 ， 比 如 主题 、 服 务 和 动作 库 等 。 


SMACH 库 的 核心 是 精简 的 ， 并 且 增 加 了 日 志和 实用 函数 ， 提 供 了 状态 和 容器 接口 。 
2.SMACH 特 点 
SMACH 有 利于 控制 机 器 人 复杂 动作 的 具体 实现 。 在 SMACH 中 ， 所 有 可 能 的 状态 和 状态 转移 都 能 够 明确 定义 。SMACH 具 有 以 下 特点 。 
- 快速 原型 : 利用 基于 Python 的 状态 机 语法 ， 能 够 快速 构建 可 执行 的 状态 机 。 
- 复杂 状态 机 : SMACH 支 持 设计 、 维 护 和 调试 大 型 的 复杂 的 层次 状态 机 。 
自省: SMACH 可 以 创建 状态 机 的 自省 程序 ， 包 括 状态 转移 、 用 户 数 据 等 要 素 的 初始 化 。 
由 于 SMACH 和 旨 在 提供 任务 级 的 状态 机 描述 ， 因 此 其 并 不 适用 于 无 结构 性 的 任务 ， 以 及 需要 较 高 执行 效率 的 底层 系统 。 
3.SMACH 设 计 的 相关 概念 
(1) 输出 语义 


对 于 状态 机 容器 ， 其 中 的 状态 接口 都 是 通过 状态 输出 来 定义 的 。 状 态 输 出 类 型 是 状态 的 一 个 属性 ， 并 且 必 须 在 执行 前 明确 声明 。 不 同类 型 的 容器 中 ， 不 同 状态 输出 会 触发 不 同 的 事件 ， 当 状态 输出 确定 
之 后 ， 从 状态 的 视角 来 看 后 续 的 事件 是 不 相关 的 ， 所 以 对 于 给 定 的 状态 而 言 ， 输 出 可 以 认为 是 “局 部 ”数据 。 


状态 能 够 提供 “成 功 ”、“ 失 败 ”、“ 抢 占 ” 等 类 型 的 输出 ， 对 于 状态 而 言 ， 这 些 输出 是 状态 之 间 的 交互 方式 ;对 于 状态 机 而 言 ， 这 些 输出 可 能 和 其 他 的 状态 相 联系 ， 从 而 导致 状态 间 转 移 ， 对 于 容器 
而 言 ， 输 出 会 有 不 同 的 处 理 方式 ， 可 能 仅仅 是 状态 和 容器 之 间 的 普通 接口 。 


(2) 用 户 数据 


容器 包含 对 应 的 平面 数据 库 ， 用 于 不 同 状 态 之 间 的 数据 传递 ， 可 以 存储 状态 生成 的 某 些 中 间 结 果 或 是 传感器 返回 的 传 感 数据 。 数 据 库 支持 在 状态 执行 过 程 中 保存 修改 的 数据 ， 而 且 这 些 数据 可 以 被 其 他 
任务 或 进程 利用 。 应 用 可 以 通过 状态 的 输入 索引 和 输出 索引 访问 用 户 数据 ， 输 入 索引 和 输出 索引 是 状态 的 属性 ， 并 且 必 须 在 执行 前 被 声明 ， 以 防止 在 调试 或 设计 复杂 状态 机 时 出 现 错误 。 


用 户 数据 通常 是 和 单一 的 容器 相关 联 的 ， 容 器 均 会 有 对 应 的 用 户 数据 结构 ， 从 而 实现 容器 所 包含 子 状态 的 数据 访问 、 数 据 共 享 及 状态 间 通 信 等 。 用 户 数据 也 可 以 通过 接口 函数 传递 给 其 他 关联 的 容器 。 
(3) 抢占 


SMACH 也 提供 了 抢占 的 概念 ， 状 态 利 用 接口 来 处 理 容 器 ， 以 及 其 所 含 状态 的 抢占 请 求 。 不 同 的 容器 类 型 对 应 不 同 的 行为 来 回应 抢占 请 求 。 因 此 ， 系 统 不 仪 能 够 对 某 一 结束 操作 的 用 户 信号 给 出 明确 响 
应 ， 同 时 也 应 能 够 通过 更 高 层 的 程序 化 的 操作 取消 某 些 动作 。 


抢占 是 SMACH 中 的 一 个 重要 概念 ， 在 状态 的 执行 过 程 中 要 不 断 检查 状态 抢占 标志 位 。 尤 其 是 在 并 发 的 容器 中 ， 抢 占 显得 尤为 重要 ， 当 某 个 并 发 的 分 支 终结 时 ， 也 就 意味 着 并 发 容器 执行 的 结束 ， 那 么 
当前 正在 执行 的 其 他 分 支 状态 都 要 被 抢占 。 


(4) Be 


SMACH 容 器 提供 了 调试 接口 ， 支 持 开 发 者 通过 SMACH 树 形 结构 来 设置 所 有 的 初始 状态 和 用 户 数据 。 此 外 ，SMACH 窗 口 工具 实现 了 操作 的 可 视 化 。 可 视 化 工具 包含 每 个 容器 初始 的 状态 标签 ， 以 及 每 
层 的 用 户 数据 。 自 省 操作 通过 客户 端 和 服务 器 模式 实现 ， 通 过 订阅 相关 主题 ， 实 现 客户 端 和 服务 器 的 通信 ， 以 此 来 实现 容器 的 用 户 数据 和 状态 的 初始 化 操作 。 


5.2.3 SMACH 状 态 摘 述 


1. 状 态 (State) 及 其 属性 
(1) 设计 思想 


State 是 SMACH 的 状态 基 类 。 所 有 状态 和 容器 都 是 从 State 继 承 而 来 的 ， 而 且 在 状态 机 的 设计 和 执行 过 程 中 均 是 以 状态 State 为 单位 来 处 理 的 。State 定 义 了 状态 的 接口 ， 状 态 通 过 两 种 方式 与 容器 进行 交 
互 ， 第 一 种 方式 是 通过 状态 输出 ， 第 二 种 方式 是 在 运行 时 读 出 或 写 入 用 户 数 据 。 这 些 交 互 方式 必须 在 状态 执行 前 声明 ， 并 且 在 构建 时 进行 一 致 性 检查 。 状 态 实 现 了 阻塞 的 execute () 函数 ， 只 有 在 状态 返 
回 给 定 输出 时 ， 才 会 终止 状态 的 执行 。 


(2) 基本 属性 


状态 内 置 成 员 函 数 主 要 包括 init_ () 和 execute () 。 其 中 _init () 函数 主要 用 于 状态 的 初始 化 ， 包 括 参 数 存 储 ， 输 出 声明 以 及 状态 转移 。execute () 方法 在 状态 执行 时 调用 ， 可 以 根据 具体 需要 实 
现 不 同 的 功能 。 


(3) 重要 属性 
抢占 是 state 中 的 重要 概念 ， 定 义 一 个 状态 变量 标识 状态 的 抢占 状况 ， 通 过 内 置 的 成 员 函 数 实 现 对 抢占 标识 的 访问 。 表 5-4 中 所 列 的 函数 通过 维护 特殊 的 标志 位 来 指明 状态 的 抢占 情况 。 


表 5-4 抢占 属性 重要 函数 


函数 名 称 实现 方式 
request_preempt 设置 抢占 标志 位 preempt requested 为 True， 抢 占 该 状态 
service preempt 没 置 抢占 标志 位 preempt_requested 为 False， 表 明 抢 占 生 效 ， 但 执行 该 隔 
| 数 后 ， 状 态 可 再 次 被 抢占 
recall preempt 设置 抢占 标志 位 preempt requested 为 False， 解 除 抢占 
preempt_requested HE SK Ae fF eae A. MPR ee A, MU eK GR [A] True 


通过 以 上 四 个 函数 ， 每 个 状态 都 可 以 维护 自己 的 抢占 标志 位 ， 一 旦 状态 被 抢占 ， 它 并 不 是 马上 停止 执行 ， 而 是 在 执行 到 抢占 检测 点 时 ， 才 会 判断 标志 位 ， 状 态 执行 才 会 结束 。 


2. 状 态 (State) 分 类 


State 可 被 细 分 为 以 下 6 种 状态 类 别 ， 具 体 如 下 。 
(1) 通用 状态 (Generic State) 


该 状态 是 从 State 状 态 继承 而 来 的 ， 所 以 不 必定 义 基本 状态 接口 。Generic state 并 不 属于 SMACH 提 供 的 基本 状态 ， 之 所 以 在 这 里 提出 这 个 概念 ， 是 因为 它 在 做 数值 运算 和 其 他 单一 的 非 功 能 性 调用 时 非 
常 重 要 ， 不 必 再 单独 重 写 代 码 来 实现 与 状态 机 的 交互 ， 仅 仪 重 载 它 的 execute () 就 可 以 实现 相关 功能 ， 这 样 做 不 仅 提 高 了 代码 的 复 用 ， 也 增强 了 代码 的 清晰 度 。 设 计 完善 的 状态 不 会 在 创建 时 被 阻塞 ， 如 
果 需 要 在 创建 时 等 待 其 他 的 状态 完成 ， 可 以 用 多 线程 的 方法 实现 该 状态 。 


(2) 回调 状态 (CBState) 


顾名思义 ， 回 调 状态 主要 是 把 回调 函数 封装 成 状态 ， 其 继承 了 状态 的 基本 属性 。 执 行 该 状态 时 仪 执行 单一 的 函数 调用 即 可 ， 可 以 通过 smach.cb_interface 拓 展 接口 的 功能 ， 其 作用 是 在 状态 输出 之 后 拓 
展 状态 的 输出 语义 ， 也 就 是 使 得 状态 能 够 具有 附加 的 属性 ， 从 这 个 意义 上 说 ， 该 接口 就 是 回调 函数 ， 不 过 这 个 回调 函数 需要 在 构建 时 定义 。CBstate 调 用 时 至 少 要 传递 一 个 名 为 用 户 数据 (userdata) 的 参 
数 ， 附 加 的 参数 可 以 在 构建 时 给 出 ， 这 些 参数 将 在 状态 执行 时 传递 给 回调 函数 。 


(3) 监控 状态 (MonitorState) 
该 状态 主要 是 通过 订阅 给 定 的 主题 来 实现 对 某 一 主题 的 监控 ， 在 每 次 接收 到 主题 时 执行 用 户 自 定义 的 回调 函数 。 


该 状态 通过 条 件 函 数 检测 给 定 的 主题 。 通 过 订阅 相关 主题 ， 利 用 python 的 线程 概念 使 其 阻塞 ， 阻 塞 时 不 允许 抢占 出 现 。 当 订阅 的 主题 消息 出 现时 ， 判 断 函 数 调用 和 检查 次 数 等 条 件 ， 条 件 成 立时 则 唤醒 
线程 ， 取 消 订阅 该 主题 ， 具 体 流程 如 图 5-12 所 示 。 


(4) 条 件 状 态 (ConditionState) 


该 状态 主要 通过 函数 调用 实现 对 某 个 条 件 的 多 次 检查 。 当 参数 中 的 max_checks 大 于 1 时 ， 状 态 就 循环 检测 某 个 条 件 。 其 在 循环 过 程 中 可 以 被 抢占 ， 一 旦 状态 被 抢占 ， 就 终止 状态 执行 。 当 条 件 为 False， 
或 者 检查 次 数 超过 最 大 检查 次 数 时 ， 则 表明 某 个 条 件 未 能 满足 ， 状 态 返 回 False， 具 体 流程 如 图 5-13 所 示 。 
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图 5-13 条件 状态 示意 图 


(5) 简单 动作 状态 (SimpleActionState ) 


该 状态 的 创建 主要 是 封装 actionlib 中 的 动作 ， 虽 然 可 以 从 一 般 的 程序 中 调用 actionlib 中 的 动作 ， 但 是 需要 编写 比较 明确 的 动作 调用 代码 ， 由 于 SMACH 已 经 明确 实现 了 动作 状态 的 调用 ， 因 此 实现 动作 
调用 更 加 简单 ， 而 且 更 节省 代码 。SMACH 提 供 了 作为 actionlib 动 作 代理 服务 器 的 状态 类 ， 实 现 了 某 个 节点 的 具体 动作 。 在 传递 参数 时 ， 只 需要 提供 具体 动作 目标 参数 ， 状 态 执行 时 就 会 通过 传递 参数 来 生 


成 具体 的 动作 目标 ， 通 过 SendGoal 发 送 目标 消息 ， 对 应 的 节点 就 会 通过 目标 消息 实现 该 动作 。 在 实例 化 时 需要 包含 主题 名 称 、 动 作 类 型 和 生成 目标 的 参数 ， 简 单 动作 状态 的 输出 是 成 功 、 失 败 或 抢占 。 
动作 状态 实现 
在 状态 初始 过 程 中 ， 首 先 调 用 actionlib， 创 建 SmpleActionClient， 判 断 是 否 连 接 服务 器 ， 利 用 用 户 数据 生成 目标 ， 然 后 利用 接口 函数 SendGoal 发 布 相关 主题 ， 启 动 计 时 器 ， 阻 塞 状 态 ， 要 求 在 规定 时 


间 内 完成 ， 否 则 取消 目标 执行 。 结 果 调用 返回 时 ， 唤 醒 状 态 继续 执 行 ， 生 成 的 结果 放 在 用 户 数据 域 。 具 体 处 理 过 程 如 图 5-14 所 示 。 
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图 5-14 ”简单 动作 状态 示意 图 


SimpleActionClient 中 订阅 和 发 布 的 相关 主题 : 














self.pub_goal = rospy.Publisher (rospy.remap name (ns) + '/goal', 
self.ActionGoal, ) 
self.pub cancel = rospy.Publisher (rospy.remap name (ns) + '/cancel', 




















GoalID, ) 

self.status sub = rospy.Subscriber (rospy.remap name (ns) + '/status', 
GoalStatusArray, self. status cb) 7 

self.result_ sub = rospy.Subscriber (rospy.remap name (ns) + '/result', 
self.ActionResult, self. result cb) 

self.feedback sub = rospy.Subscriber (rospy.remap name (ns) + '/feedback', 
self.ActionFeedback, self. feedback cb) 



















































































actionlib 中 Client 和 Server 的 交互 方式 如 图 5-15 所 示 。 
Client Application Server Application 


uset code user code 


function calls Action 
client.sendGoal (...) 
Server 


callbacks 


callbacks 


function calls 





图 5-15 Client 和 Setvet 交 互 方式 示意 图 
@ 动 作 消息 定义 
目标 消息 定义 如 下 。 
. 空 目标 消息 : 调用 动作 服务 器 不 需要 在 目标 消息 中 传递 任何 数据 。 
` 确定 目标 消息 : 用 户 指 定 消息 内 容 ， 传递 给 服务 器 的 确定 目标 消息 。 
APR ARE: 用 户 数据 域 中 包含 了 所 有 生成 目标 消息 的 信息 ， 可 以 把 用 户 数据 域 和 目标 消息 连接 起 来 生成 目标 消息 。 
- 目标 消息 调用 : 可 以 创建 目标 消息 调用 ， 在 目标 消息 调用 中 设计 目标 消息 ， 并 可 使 用 输入 索引 来 访问 用 户 数据 ， 提 取 生 成 目标 消息 的 数据 ， 生 成 目标 消息 。 
结果 消息 定义 如 下 。 
- 用 户 数 据 域 存储 结果 消息 : 动作 执行 完成 后 ， 将 修改 后 的 数据 存放 在 状态 的 用 户 数据 域 中 。 


. 结果 调用 : 结果 调用 和 目标 调用 很 相似 。 可 以 从 动作 的 结果 消息 中 读 取 任 何 数据 ， 甚 至 可 以 返回 不 同 的 输出 ， 而 不 仅仅 是 成 功 、 失 败 或 抢占 。 在 结果 调用 中 ， 可 以 获得 动作 执行 完成 的 状态 ， 从 而 获 
知 动作 执行 是 成 功 、 失 败 还 是 被 抢占 。 同 时 还 可 以 访问 用 户 数据 ， 得 到 动作 的 结果 ， 或 者 选择 从 结果 调用 中 返回 不 同 的 输出 。 如 果 状 态 未 返回 任何 结果 ， 则 直接 返回 动作 执行 的 相应 输出 。 


(6) 服务 状态 (ServiceState) 


该 状态 用 来 封装 节点 提供 的 服务 。 虽 然 一 般 可 以 从 给 定 程 序 中 调用 服务 ， 但 需要 明确 服务 调用 的 过 程 ， 这 一 点 对 于 不 太 清楚 服务 调用 的 编程 人 员 而 言 会 比较 麻烦 。 因 此 ，SMACH 通 过 把 服务 封装 成 状 
态 ， 支 持 服务 调用 ， 从 而 使 得 服务 调用 变 得 简单 。SMACH 提 供 了 服务 状态 ， 同 样 在 该 状态 创建 时 ， 需 要 request 和 result 参 数 ， 其 分 别 代表 服务 的 请 求 消息 和 结果 消息 ， 在 状态 执行 时 会 根据 传递 的 请 求 参 
数 生成 请 求 消息 ， 在 状态 完成 时 会 接收 结果 消息 并 存储 在 用 户 数据 中 。 在 实例 化 该 状态 时 ， 需 要 具体 给 出 服务 名 称 、 服 务 类 型 和 生成 请 求 的 参数 。 服 务 器 的 状态 输出 可 以 是 成 功 、 失 败 或 抢占 。 


@ 代 码 实 现 要 点 


. 创建 代理 服务 器 。 














self. proxy = rospy.ServiceProxy(self. service name, self. service spec) 











创建 代理 的 服务 器 可 接收 请 求 消息 ， 执 行 请 求 消息 。 












































. 生成 请 求 消息 

request update self. request cb(smach.Remapper (ud, 
self. request cb input keys, | 

self. request cb output keys, []), 

self. request, 

*self. request cb args, 

**xself. request cb kwargs) 


如 果 定 义 了 请 求 消息 调用 ， 则 该 调用 通过 传 入 的 用 户 数据 ， 生 成 请 求 消息 。 











self. response = self. proxy(self. request) 


. 生成 应 答 结果 


response cb outcome = self. response cb (smach.Remapper (ud, 
self. response cb input keys, 

self. response cb output_keys, []) , self. response, 
*self. response cb args, 

**self. response cb kwargs) 






































在 执行 请 求 结束 且 返 回 目标 消息 后 ， 可 以 通过 回调 函数 访问 结果 消息 ， 提 取 所 需要 的 数据 并 人 存储 到 用 户 数据 域 中 。 
@ 请 求 消息 定义 
. 空 请 求 消息 : 调用 服务 器 不 需要 在 请 求 消息 中 传递 任何 数据 。 
- 确定 请 求 消息 : 明确 地 指明 传递 给 服务 器 的 请 求 消息 。 
自 


* 用 户 数据 请 求 消息 : 用 户 数据 域 中 存储 了 生成 请 求 消息 所 需 的 所 有 数据 ， 可 以 简单 地 把 用 户 数据 域 连接 到 请 求 消息 。 


* 请 求 调用 : 创建 请 求 消息 调用 ， 在 调用 中 形成 请 求 消息 。 并 可 使 用 输入 索引 访问 用 户 数 据 ， 提 取 目 标 消 息 的 数据 ， 生 成 目标 消息 。 


:应答 消息 存储 到 用 户 数据 : 服务 应 答 消 息 的 数据 存放 在 状态 的 用 户 数据 中 。 


. 应 答 调用 : 应 答 调用 和 请 求 调用 很 相似 。 可 以 从 动作 的 应 答 消息 域 中 读 取 任 何 数据 ， 甚 至 是 返回 不 同 的 输出 而 不 是 成 功 、 失 败 或 抢占 。 在 应 答 调用 中 ， 可 以 得 到 服务 调用 的 状态 ， 获 知 服务 是 执行 成 
功 、 失 败 还 是 被 抢占 。 同 时 还 可 以 访问 用 户 数据 域 得 到 动作 应 答 ， 或 者 从 应 答 调用 中 返回 不 同 的 输出 。 如 果 状 态 没有 返回 任何 应 答 ， 则 直接 返回 服务 相应 的 输出 。 


5.2.4 ”SMACH 容 器 设计 与 实现 


1. 容 器 (Container) 设计 


器 由 State 类 继承 而 来 ， 具 有 State 类 的 基本 属性 。 容 器 是 一 个 或 多 个 状态 的 集合 ， 实 现 了 不 同 状态 的 组 合 ， 而 不 同 的 状态 组 合 则 对 应 了 不 同 的 执行 方式 。SMACH 容 器 类 中 包含 获取 、 添 加 及 设置 状态 


容 
等 成 员 函 数 ， 同 时 还 提供 了 执行 、 状 态 转移 和 终结 调用 等 方法 。Python 的 with 关键 字 用 于 向 容器 中 添加 状态 ， 可 以 通过 调用 容器 的 静态 方法 添加 子 状 态 ， 通 过 确定 当前 处 于 开启 状态 的 容器 ， 依 次 添加 相应 
的 状态 到 该 容器 中 。 


(1) 回调 函数 语义 

“ Start: 进入 容器 时 调用 。 

Transition: 容器 的 状态 发 生变 化 时 调用 。 

Tetmination: 容器 执行 结束 时 调用 。 

值得 注意 的 是 ， 容 器 的 执行 取决 于 调用 相关 的 回调 立 数 的 时 机 及 传 入 的 参数 类 型 。 
SMACH 提 供 了 多 种 不 同类 型 的 容器 ， 不 同 的 容器 具有 不 同 的 执行 语义 。 容 器 需要 明确 地 指明 所 包含 状态 的 状态 转移 方式 。 
SMACH 容 器 具有 以 下 共同 属性 。 

` 状态 字典 : 在 字典 中 状态 对 象 均 是 通过 对 应 字符 串 的 标签 来 索引 的 。 

“ 用 户 数据: 可 以 被 其 所 有 的 子 状态 访问 的 用 户 数据 。 

“ 开启 标识 : 容器 能 否 被 修改 的 标识 。 

(2) 容器 的 基本 操作 


SMACH 为 容器 提供 了 一 组 操作 函数 ， 具 体 如 表 5-5 所 示 。 


RZ ER 作用 


getitem (self, key) 通过 索引 来 访问 对 应 状态 

get children(self) DRAG EE fie PTA POR AS 

check consistency(self) 4S ie — BPE fs 

copy input keys (self, parent ud, ud) 日 动 数据 传递 ， 从 parent ud 到 ud 
copy output keys(self, ud, parent ud) 目 动 数据 传递 ， 从 ud 到 parent ud 


2. 并 发 容器 (Concurrence) 
顾名思义 ， 并 发 容器 是 指 添加 到 容器 中 的 状态 可 以 同时 执行 。 当 包含 的 所 有 状态 都 处 于 状态 转移 的 ready 状 态 时 ， 这 些 并 发 的 状态 就 会 同时 开始 执行 ， 进 入 状态 转移 阶段 。 
容器 可 以 返回 给 定 的 结果 ， 该 结果 就 是 它 所 包含 的 子 状态 输出 结果 中 的 某 一 个 输出 。 返 回 的 结果 应 该 在 容器 创建 时 指明 ， 或 者 在 创建 之 后 添加 到 输出 映射 中 。 
只 有 当 其 包含 的 所 有 状态 都 终结 时 ， 并 发 容器 才 会 终结 。 容 器 执行 结束 时 可 能 某 些 状态 的 分 支 会 因 被 抢占 而 终止 执行 。 
(1) 并 发 容器 执行 结束 的 条 件 
容器 执行 结束 至 少 应 满足 下 列 条 件 之 一 。 
“ 所 有 子 状态 执行 结束 。 


.至少 一 个 子 状态 执行 结束 。 


“ 用 户 定义 的 回调 函数 执行 结束 。 
对 应 不 同 容器 执行 的 结束 方式 ， 输 出 结果 也 可 以 有 四 种 不 同 的 方式 。 
“ 用 户 定 义 的 回调 函数 返回 一 个 输出 。 
:一 个 需要 多 个 子 状 态 终结 的 子 输出 映射 被 满足 。 
一 个 只 需要 一 个 子 状态 终结 的 子 映射 被 满足 。 
` 没有 输出 映射 被 满足 ， 返 回 默 认输 出 。 


需要 指出 的 是 ， 输 出 映射 的 详细 参数 和 输出 回调 函数 均 应 该 在 创建 时 就 指定 。 应 用 可 以 不 止 一 个 输出 方案 ， 每 一 个 输出 方案 均 有 不 被 满足 的 可 能 性 。 在 设 定 了 多 种 输出 方案 的 情况 下 ， 多 个 给 定 的 输出 
方案 被 满足 时 ， 输 出 选择 的 优先 级 由 高 到 低 排 列 如 下 : 回调 函数 一 第 一 个 触发 的 输出 映射 一 最 后 触发 的 输出 映射 一 默认 输出 ， 状 态 执行 结束 时 ， 具 体 的 输出 会 参考 输出 映射 或 回调 函数 。 
(2) 输出 映射 


输出 映射 是 决定 容器 输出 的 重要 依据 。 输 出 映射 可 使 不 同 子 状态 的 输出 实现 不 同 的 组 合 ， 形 成 不 同 的 子 映 射 。 每 一 个 容器 的 输出 对 应 一 个 字典 ， 该 字典 是 子 状态 标签 到 输出 的 映射 。 如 果 没 有 子 输出 映 
射 被 满足 ， 那 么 并 发 容器 将 会 以 默认 输出 终结 。 下 面 给 出 一 个 示例 : 


{'succeeded' : {'FOO':'succeeded', 'BAR':'done'}, 
‘aborted' : {'FOO':'aborted'}} 











如 果 状 态 ‘FOO” 和 ‘BAR’ LA ‘succeeded’ #1 ‘done’ 状态 终结 ， 那 么 并 发 容器 将 以 “succeeded” 作为 输出 。 如 果 状 态 FOO 返回 输出 ‘aborted’ ， 那 么 并 发 容器 将 以 “aborted” 作为 输 
出 。 如 果 一 个 状态 的 输出 结果 在 并 发 容器 中 没有 明确 指明 ， 那 么 该 输出 与 并 发 容器 输出 不 相关 。 


如 果 某 个 子 输出 映射 的 输出 条 件 是 另 一 个 子 输出 映射 输出 条 件 的 子 集 ， 那 么 容器 将 会 选择 更 多 的 输出 条 件 被 满足 的 子 输出 作为 最 终 的 输出 。SMACH 中 并 没有 明确 定义 两 个 输出 条 件 相同 的 情况 。 如 果 
用 户 需 要 更 加 复杂 的 输出 方案 ， 那 么 可 以 采用 输出 回调 函数 对 输出 进行 更 为 复杂 的 摘 述 。 


(3) 子 状态 终结 回调 函数 

该 回调 函数 将 在 子 状 态 执行 结束 时 调用 ， 有 些 子 状 态 集合 在 执行 结束 时 需要 抢占 其 他 正在 执行 的 状态 ， 以 终止 其 执行 ， 这 是 并 发 容器 常见 的 情况 。 当 某 个 子 状 态 终结 时 ， 回 调 函 数 被 调用 ， 其 传递 单一 
的 参数 ， 该 参数 反映 了 子 状态 标签 到 输出 结果 的 字典 映射 。 如 果 一 个 子 状 态 还 没有 终结 ， 那 么 它 在 字典 中 的 输出 值 将 会 是 None。 

回调 函数 返回 值 有 以 下 三 种 类 型 。 

False: 继续 阻塞 容器 直到 其 他 的 状态 终结 。 

True: 抢占 其 他 的 状态 。 

:状态 标签 列表 : 指明 要 抢占 的 指定 状态 。 

(4) 输出 回调 函数 

如 果 输 出 形式 比较 复杂 ， 那 么 用 户 可 以 采用 输出 回调 函数 指定 容器 的 输出 。 回 调子 数 将 在 所 有 子 状态 终结 的 条 件 下 被 调用 ， 传 递 一 个 字典 的 映射 ， 该 映射 就 是 状态 标签 到 输出 的 映射 。 


如 果 回 调 函 数 返回 的 是 一 个 字符 串 ， 那 么 该 字符 串 将 会 被 认为 是 该 并 发 容器 的 输出 。 如 果 回调 函数 返回 的 是 None， 那 么 并 发 容器 将 首先 检查 输出 映射 ， 如 果 没 有 输出 映射 被 满足 的 话 ， 那 么 容器 将 返回 
一 个 默认 输出 。 图 5-16 是 并 发 容器 的 状态 示意 图 。 
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图 5-16 ConcurrencekkA REA 
(5) 具体 实现 要 点 


当 所 有 子 状态 都 添加 到 容器 后 ， 为 了 实现 并 发 性 ， 必 须 利用 Python 中 的 Threading 构 件 ， 为 每 个 并 发 的 状态 创建 单独 的 线程 : 


























for (label, state) in ((k,self. states[k]) forkinself. states): 
self. child outcomes[label] = None 
self. _ threads [label] = threading.Thread( name='concurrent split:'t+label, 





























target=self. state runner, 
args= (label, ) ) 





线程 的 具体 执行 是 通过 调用 self state_runner 函 数 来 实现 的 ， 并 且 在 执行 过 程 中 ， 如 果 某 个 线程 的 终结 引发 了 容器 的 终结 时 ， 那 么 该 线程 就 会 抢占 其 他 正在 执行 的 线程 。 该 线程 创建 后 ， 为 了 实现 线程 
间 的 同步 ， 必 须 阻塞 该 容器 ， 以 等 待 某 个 线程 结束 标志 出 现 ， 具 体 如 下 。 


- self_done_cond.acquire () 
- self._ready_event.set () 
- self._done_cond.wait () 
- self._done_cond.release () 
当 某 线程 终结 是 该 容器 的 终结 时 ， 该 线程 就 要 调用 self. done cond.notify all () 来 唤醒 等 待 该 条 件 的 线程 。 
3. 状 态 机 容器 及 其 扩展 
(1) 状态 机 容器 (StateMachine) 


该 类 定义 了 状态 机 容器 ， 由 于 继承 了 Container 类 ， 所 以 其 具有 Container 类 的 基本 属性 。 状 态 机 容器 是 最 常用 的 容器 ， 因 为 在 设计 状态 机 时 ， 通 常 将 它 作 为 最 外 层 的 容器 ， 所 有 的 状态 都 作为 其 子 状 
添加 在 StateMachine 中 的 状态 具有 三 元 组 的 属性 。 


Ot 


~ 


- 标签 (label) 
-IKA KA (stateinstance) 
状态 转移 (transition) 


是 字符 串 类 型 ， 状 态 类 型 是 任意 的 状态 类 ， 可 以 是 SMACH 提 供 的 状态 类 ， 或 者 是 用 户 自己 定义 的 实现 特定 功能 的 状态 类 ， 而 状态 转移 是 一 个 字典 映射 ， 字 典 是 字符 串 对 所 转移 新 状态 的 字符 串 映 
射 ， 其 决定 了 状态 机 中 状态 执行 结束 时 后 续 执 行 的 状态 。 状 态 转移 有 三 种 形式 ， 具 体 如 下 。 


- OUTCOME—STATE_LABEL 


OUTCOME 一 Noneotunspecified 





- OUTCOMESM_OUTCOME 
状态 添加 进 容器 时 ， 利 用 add_auto () 可 以 自动 连接 两 个 转移 的 状态 。 添 加 的 每 一 个 状态 均 可 接收 到 一 个 附加 的 状态 转移 ， 其 是 当前 状态 到 下 一 个 添加 的 状态 。 


值得 ，stateMachine 中 还 有 一 个 重要 的 函数 update_once () ， 这 个 函数 实现 了 一 次 性 完成 状态 机 中 状态 的 执行 。 通 过 检查 当前 状态 ， 如 果 满 足 状 态 转移 ， 那 么 当前 状态 会 自动 终止 ， 然 后 
从 当前 状态 的 转移 字典 中 提取 下 一 个 将 要 执行 的 状态 标签 ， 转 移 至 下 一 状态 ， 直 至 状态 机 执行 结束 。 


(2) 顺序 状态 机 容器 (Sequence) 


顺序 状态 机 容器 在 继承 StateMachine 的 基础 上 ， 拓 展 了 StateMachine 的 功能 。Sequence 可 以 根据 添加 状态 的 顺序 自动 连接 相应 的 状态 。 并 且 按 照 连接 好 的 状态 顺序 执行 。 Sequence 状 态 机 容器 拥有 
自己 的 输出 ， 而 且 输 出 必须 在 创建 时 指明 。 


顺序 状态 机 容器 的 添加 状态 和 上 述 容器 状态 添加 的 方法 相同 ， 但 是 每 个 要 添加 的 状态 都 会 接收 到 一 个 附加 的 状态 转移 ， 该 状态 转移 是 指 从 当前 状态 到 下 一 个 需要 添加 的 状态 的 转移 。 
(3) ARASI (Iterator) 


迭代 状态 机 容器 继承 了 stateMachine 的 基本 功能 ， 对 stateMachine 的 扩展 主要 是 可 根据 添加 状态 的 顺序 自动 连接 相应 的 状态 ， 并 且 依 据 状 态 的 顺序 自动 执行 的 同时 能 根据 用 户 提供 的 数据 进行 状态 的 
和 迭代， 直到 出 现 某 个 输出 结果 时 才 会 终止 执行 。 具 体 实 现 包含 以 下 三 个 要 素 。 


` 循环 输出 : 包含 状态 输出 的 列表 ， 一 旦 菜 个 输出 是 这 个 列表 中 的 元 素 ， 那 么 迭代 状态 机 容器 会 继续 执行 。 如 果 列 表 为 空 ， 所 有 输出 又 不 在 中 断 输出 列表 中 时 ， 迁 代 状 态 机 容器 还 是 会 继续 执行 。 如 果 
循环 输出 列表 和 中 断 输 出 列表 包含 了 相同 的 元 素 ， 那 么 中 断 输 出 列表 的 优先 级 更 高 。 


.中断 输出 : 包含 状态 输出 的 列表 ， 一 旦 某 个 输出 是 列表 中 的 元 素 ， 那 么 迭代 状态 机 容器 就 会 中 断 执行 ， 并 且 返 回 这 一 输出 ， 或 者 把 该 输出 索引 到 最 终 的 输出 映射 上 。 
最 终 输出 映射 : 包含 状态 输出 到 状态 机 容器 输出 的 映射 : 在 选 代 状 态 机 容器 结束 执行 时 ， 该 映射 可 用 来 转换 状态 输出 到 状态 机 容器 输出 ， 没 有 定义 的 状态 输出 默认 是 状态 机 容器 的 输出 。 


需要 注意 的 是 耗 尽 输出 的 特殊 情况 ， 在 所 有 用 户 数据 耗 尽 的 情况 下 ， 人 迭代 状态 机 容器 仍 没 有 中 断 执 行 ， 那 么 迭代 状态 机 容器 就 会 输出 这 一 结果 。 


5.2.5 ”SMACH 主 要 处 理 构件 分 析 


1. 动 作 服 务 器 封装 (ActionServerWrapper) 


该 类 实现 的 功能 是 实现 动作 服务 器 和 状态 机 的 封装 ， 用 关联 状态 机 的 形式 实现 动作 服务 器 的 功能 ， 其 属于 状态 机 顶层 的 设计 构件 ， 而 不 属于 状态 。 我 们 可 以 定义 专用 的 状态 机 ， 通 过 此 类 完成 封装 ， 作 
为 动作 库 中 具体 的 动作 服务 器 


具体 设计 时 ， 首 先 需要 创建 一 个 简单 的 动作 服务 器 








self. action server = SimpleActionServer (self. server name, 
self. action | Spec, 

execute cb=self .execute ch, 

auto | start=False) 



































一 旦 接收 到 动作 服务 器 的 目标 消息 后 ， 即 可 通过 调用 回调 函数 self. execute_cb， 使 关联 的 状态 机 完成 具体 的 目标 任务 : 





container outcome = self.wrapped_ container.execute (smach.Remapper ( 
self.userdata, 

self.wrapped container.get registered input keys (), 

self.wrapped container.get registered output keys(), 


{})) 









































执行 结束 后 ， 返 回执 行 结果 (succeeded、preempted 或 aborted) ， 用 户 可 以 指定 输出 列表 ， 动 作 服务 器 根据 状态 机 的 输出 为 具体 的 目标 任务 返回 执行 结果 。 动 作 服务 器 的 封装 如 图 5-17 所 示 。 







传 入 参数 : 服务 器 名 称 
coe | A ARS 
创建 动作 服务 器 关联 状态 机 





接 动 作 目 标 ， 存 
储 到 user data 中 


图 5-17 动作 服务 器 封装 示意 图 
2. 自 省 客户 端 (IntrospectionClient) 


自省 客户 端 用 于 初始 化 状态 机 。 通 过 树 形 的 递归 结构 ， 订 阅 相关 的 主题 ， 可 以 以 递归 的 方式 初始 化 状态 机 的 所 有 状态 和 用 户 数据 ， 并 且 可 以 接收 初始 化 的 当前 进度 和 状态 。 其 中 的 三 种 主题 类 型 如 表 5-6 
所 示 。 


表 5-6 ”自省 主题 消息 


主题 名 称 作用 
STATUS TOPIC BRM SA AE Ar HAR AS 
INIT TOPIC 初始 化 的 主题 
STRUCTURE TOPIC 获取 当前 初始 化 的 容 硕 结构 


3. 自 省 服务 器 (IntrospectionServer) 
自省 服务 器 主要 用 于 接收 来 自 自省 客户 端 发 送 的 主题 消息 ， 然 后 通过 递归 的 方式 创建 容器 的 代理 服务 ， 实 现 容器 状态 和 用 户 数据 的 初始 化 ， 同 时 创建 相关 的 发 布 者 ， 为 客户 端 返 回 初始 化 结果 。 
4.SMACH 中 的 工具 类 


SMACH 定 义 了 一 组 工具 类 ， 其 不 是 状态 类 ， 而 是 支持 状态 类 的 具体 实现 。 用 户 数据 UserData 类 不 仅 实 现 了 数据 存储 和 数据 操作 ， 而 且 可 使 容器 的 数据 得 以 被 子 状态 共享 。 Remapper 类 实际 上 实现 了 
状态 的 输入 输出 和 用 户 数据 间 的 关联 ， 而 CBlnterface 通 过 函数 调用 拓展 了 输出 的 语义 。 工 具 类 的 具体 定义 如 表 5-7 所 示 。 


表 5-7 SMACH 工 具 类 
类 名 作用 
UserData SMACH 的 用 户 数 据 结 术 
Const 创建 用 户 指定 的 常量 
Remapper 用 户 效 据 编 构 索 引 重 映射 
cb interface&CBlInterface 用 户 效 据 和 输出 的 扩展 接口 


5.2.6 ”状态 机 的 具体 实现 


1. 状 态 创建 


创建 的 状态 应 继承 状态 基 类 (smach.State) ， 然 后 调用 state.execute 方 法 ， 实 现 具体 的 创建 操作 。 当 然 也 可 以 直接 使 用 MACH 提 供 的 状态 类 ， 传 入 相关 的 参数 ， 而 不 必 创建 新 的 状态 ; 





classFoo (smach .State) : 
def init (self, outcomes=['outcomel', 'outcome2']) 























def execute (self, userdata): 
if xxxx : return 'outcomel' 
else : return'outcome2'! 








init 方 法 可 以 初始 化 所 设计 的 状态 ， 但 需要 注意 的 是 ， 不 能 在 init 方 法 中 阻塞 该 状态 。 如 果 状 态 需要 等 待 其 他 部 分 的 完成 ， 那 么 可 以 用 单独 的 线程 实现 状态 的 初始 化 。 
状态 的 具体 创建 功能 是 在 execute 方 法 中 实现 的 。 该 方法 可 以 添加 任意 用 户 设计 的 代码 ， 而 且 可 以 阻塞 ， 一 旦 该 方法 执行 结束 ， 状 态 执行 也 就 终结 了 。 

一 个 状态 终结 时 ， 会 返回 一 个 输出 。 每 个 状态 都 有 多 个 可 能 的 输出 ， 而 每 一 个 输出 都 是 由 用 户 定义 的 字符 串 来 描述 状态 机 的 完成 情况 。 

2. 状 态 添加 


状态 机 可 包含 多 个 状态 ， 当 添加 状态 到 状态 机 中 时 ， 需 要 指定 状态 之 间 的 转移 。 需 要 注意 的 是 ， 添 加 状态 的 输出 要 求 是 严格 的 ， 状 态 之 间 的 转移 必须 具备 一 致 性 ， 也 就 是 说 某 个 状态 执行 结束 ， 需 要 转 
是 











移 的 下 一 状态 必须 是 当前 状态 机 容器 中 的 状态 ， 或 者 是 容器 执行 的 终点 。 例 如 : 
sm = smach.StateMachine (outcomes=[' outcome4', 'outcome5']) 
withsm: 
smach.StateMachine.add('FOO', Foo(), 
transitions={'outcomel':'BAR', 














Toutcome2':'outcome4'}) 
smach.StateMachine.add('BAR', Bar(), 
transitions={ 'outcome2':'FOO"'}) 






































创建 的 状态 机 如 图 5-18 所 示 。 





-out come? 


| 

| 

| 

| 

| 

| 

| 

| 

| 

| 

| 

| 

| | 
\ ~、、 out comel 
| 

| 

| 

| 

| 

out come? 
| 

| 

| 

| 

| 


out comes | out come4 





45-18 ”状态 机 示意 图 


3. 状 态 间 的 数据 传递 


(1) 指定 用 户 数据 





状态 需要 多 个 输入 数据 来 实现 特定 功能 或 可 以 将 一 些 数据 输出 到 其 他 的 状态 中 去 。 输 入 和 输出 数据 就 是 状态 的 用 户 数据 。 当 构建 一 个 状态 时 ， 应 指定 状态 的 用 户 数据 ， 同 时 需要 提供 输入 和 输出 数据 索 


引 。 


input_keys 列 表 枚 举 了 状态 执行 时 所 需要 的 所 有 输入 索引 。 这 些 数 据 索 引 应 该 在 用 户 数据 中 声明 ，execute 方 法 拥有 用 户 数据 结构 的 副本 ， 所 以 状态 可 以 从 用 户 数据 中 读 取 input_keys 中 枚 举 索引 的 数 


据 ， 但 不 能 写 入 。 
output_keys 枚 举 了 状态 提供 的 所 有 输出 索引 。 状 态 可 以 通过 输出 索引 将 数据 写 入 到 userdata 中 。 
(2) 连接 用 户 数据 
当 添 加 状态 到 状态 机 后 ， 还 需要 把 这 些 状态 的 用 户 数据 关联 起 来 ， 以 实现 状态 之 间 的 用 户 数据 传递 。 
> 状态 之 间 的 数据 传递 


重 映 射 机 制 remapping 实 现 了 ‘FOO’ 到 ‘BAR’ 状态 之 间 的 数据 传递 。 











FOO: remapping={'foo output':'sm user data'} 














BAR: remapping={'bar input':'sm user data'} 


» 状态 和 状态 机 之 间 的 数据 传递 


BAR 状 态 及 包含 BAR 状 态 的 状态 机 数据 传递 仍然 可 以 利用 重 映射 机 制 来 实现 。 例 如 ， “sm_output ”是 状态 机 的 输出 。 


WW 








AR: remapping={'bar output':'sm output'} 





还 可 以 实现 状态 机 到 状态 的 数据 传递 。 例 如 “sm_input ”是 状态 机 的 输入 索引 。 








FOO: remapping={'foo input':'sm input'} 





5.3 ”本 章 小 结 


在 ROS 系 统 中 ， 通 过 ROS 服 务 可 以 完成 客户 端 向 服务 器 端 请 求 执行 任务 ， 服 务 器 端 将 执行 结果 返回 给 客户 端 。 但 如 果 任 务 执行 时 间 比较 长 ， 这 种 情况 下 ， 客 户 端 可 


和 bE 
月 FT 


要 周期 性 地 获取 任务 执行 中 间 结 


果 , 或 者 中 断 任务 的 执行 。 因 此 ，ROS 中 提供 了 抢占 式 的 任务 调度 接口 actionlib， 该 接口 不 但 具备 任务 调度 的 能 力 ， 而 县 具备 中 断 任 务 、 任 务 状态 跟踪 与 周期 性 反馈 状态 、 执 行 过 程 中 间 结 果 反 馈 的 能 力 。 
actionlib 接 口 不 但 弥补 了 ROS 服 务 的 不 足 ， 而 且 简 单 易 用 ， 便 于 开发 。 但 actionlib 接 口 的 任务 调度 策略 比较 简单 ， 难 以 满足 多 任务 并 发 、 实 时 性 较 高 的 应 用 需求 。 


SMACH 提 供 了 构建 状态 机 必需 的 状态 和 容器 ， 使 得 智能 机 器 人 的 任务 级 设计 更 加 简便 高 效 ， 同 时 不 同 状 态 机 之 间 的 谋 套 使 得 构建 复杂 的 状态 机 也 成 为 可 能 。 设 计 人 员 根 据 智 能 机 器 人 的 具体 应 用 场景 
和 具体 任务 需要 ， 可 以 构建 相应 层次 的 状态 机 实现 具体 机 器 人 的 功能 ， 从 而 提高 代码 的 复 用 性 。 


同时 SMACH 还 提供 了 开发 状态 机 所 需要 的 相关 组 件 ， 包 括 状态 之 间 的 数据 存储 和 共享 的 UserData 类 和 封装 相关 状态 机 的 Wapper 类 等 ， 同 时 还 允许 用 户 根 据 自 己 的 需要 构建 具体 的 状态 。 


总 的 来 说 ，SMACH 是 机 器 人 方便 有 效 的 任务 级 编程 工具 ， 但 是 ， 当 状态 机 非常 复杂 时 ， 状 态 之 间 的 转移 将 会 难以 控制 ， 同 时 如 果 需 要 添加 新 的 状态 时 ， 则 要 修改 所 有 与 之 相关 的 状态 转移 ， 因 此 扩展 
性 还 需要 进一步 提升 。 


第 6 章 ”ROS 运 动 规划 及 其 实现 


6.1 智能 机 器 人 运动 控制 概述 


自主 导航 任务 是 指 智 能 机 器 人 能 够 在 一 个 已 知 或 未 知 的 环境 中 ， 在 无 人 为 操作 的 情况 下 ， 通 过 自身 传感器 感知 周围 环境 确定 自身 位 置 后 ， 以 最 少 的 代价 到 达 既 定 的 区 域 。 在 移动 机 器 人 的 自主 导航 中 ， 
自主 导航 任务 通常 包含 环境 认 知 及 自主 规划 两 方面 的 内 容 。 在 该 过 程 中 ， 运 动 规划 起 着 重要 的 作用 ， 它 确保 移动 机 器 人 能 从 起 点 到 终点 寻找 出 一 条 符合 受制 条 件 且 能 够 被 机 器 人 跟踪 的 路 径 ， 受 制 条 件 通常 


包含 诸如 运动 规律 及 其 他 适用 的 约束 ， 比 如 躲避 障碍 物 、 速 度 约 束 、 平 衡 问题 与 关节 受制 条 件 等 。 


移动 机 器 人 的 自主 规划 类 似 于 人 类 的 寻 路 行为 ， 是 在 已 认 知 的 环境 中 找到 一 条 从 起 点 到 已 知 或 未 知 终点 的 无 碰撞 且 符 合 移动 机 器 人 运动 规律 的 最 优 或 次 优 路 径 。 其 主要 研究 “Where to 
go? ”及 “How to there? ”两 个 问题 ， 即 路 径 规 划 及 基于 已 规划 路 径 的 精确 跟踪 问题 ， 如 图 6-1 所 示 。 
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图 6-1 机 器 人 运动 任务 执行 流程 


运动 规划 方法 分 为 两 个 方面 : 对 于 移动 机 器 人 ， 偏 向 于 意 指 移动 的 路 径 轨 人 迹 规划 ， 如 机 器 人 在 有 地 图 条 件 或 没有 地 图 的 条 件 下 ， 按 什么 样 的 路 径 轨迹 行走 ， 对 于 智能 工业 机 器 人 ， 则 意 指 机 械 臂 末端 行 
走 的 曲线 轨迹 ， 或 是 操作 和 臂 在 运动 过 程 中 的 位 移 、 速 度 和 加 速度 的 曲线 轮廓 。 


为 了 提高 智能 机 器 人 的 操作 效率 ， 且 高 效 完成 特定 的 任务 ， 智 能 机 器 人 必须 以 合理 的 运动 规划 为 前 提 。 运 动 规划 有 离线 和 在 线 两 种 方式 。 传 统 的 运动 规划 多 为 前 者 ， 即 事先 做 好 规划 ， 机 器 人 依 之 运 
动 ， 其 动态 适应 性 较 弱 ， 随 着 嵌入 式 计算 能 力 的 大 幅度 提升 ， 应 用 智能 需求 的 不 断 增强 ， 在 线 运 动 规划 成 为 友 展 方向 。 运 动 规划 可 进一步 细 分 为 路 径 规划 和 轨迹 规划 。 路 径 规 划 的 目标 是 使 路 径 与 障碍 物 的 
距离 尽量 远 ， 同 时 路 径 的 长 度 尽量 短 ; 轨迹 规划 的 目的 主要 是 在 机 器 人 关节 空间 移动 中 使 得 机 器 人 的 运行 时 间 尽 可 能 短 ,或 者 能 量 尽 可 能 小 。 轨 迹 规 划 在 路 径 规划 的 基础 上 加 入 了 时 间 序 列 信息 ， 对 机 器 人 
执行 任务 时 的 速度 与 加 速度 进行 规划 ， 以 满足 光滑 性 和 速度 可 控 性 等 要 求 。 


示 教 再 现 是 传统 路 径 规 划 的 实现 方法 之 一 ， 通 过 操作 空间 进行 示 教 并 记录 示 教 结果 ， 在 工作 过 程 中 加 以 复 现 ， 现 场 示 教 直接 与 机 器 人 需要 完成 的 动作 相对 应 ， 路 径直 观 且 明 确 ， 缺 点 是 需要 经 验 丰富 的 
操作 工人 ， 并 消耗 大 量 的 时 间 ， 路 径 不 一 定 是 最 优化 的 。 为 了 解决 上 述 问 题 ， 可 以 建立 机 器 人 虚拟 模型 ， 通 过 虚拟 的 可 视 化 操作 完成 对 作业 任务 的 路 径 规划 。 


路 径 规划 可 在 关节 活动 空间 中 进行 。 例 如 ， 采 用 5 次 B 样 条 对 机 器 人 的 关节 轨迹 进行 插 补 以 计算 机 器 人 各 个 关节 的 速度 、 加 速度 端点 值 ， 可 根据 平滑 性 要 求 进行 任意 配置 ， 并 将 加 速度 的 平方 相对 于 运动 
时 间 的 积分 作为 目标 遂 数 进行 优化 ， 以 确保 各 个 关节 的 运动 足够 平滑 。 另 外 ， 为 使 关节 空间 的 轨迹 规划 避免 操作 空间 的 奇异 性 问题 ， 可 采用 关节 轨迹 优化 算法 ， 其 利用 6 自由 度 机 器 人 在 任务 过 程 中 某 个 关节 
功能 上 的 元 余 ， 将 机 器 人 奇异 性 和 关节 限制 作为 约束 条 件 ， 采 用 TWA 方 法 进行 优化 计算 。 


这 里 提 到 的 “运动 规划 ” (Motion Planning) 和 “轨迹 规划 (Path Planning) 是 有 区 别 的 ; 一 般 来 说 ，Path Planning 用 于 低 维 的 、 多 运动 体 的 、 全 局 的 路 径 规 划 ， 如 无 人 车 、 无 人 机 领域 。 而 
Motion Planning 主 要 用 于 机 械 臂 、 类 人 机 器 人 领域 ， 如 工业 机 器 人 机 械 劈 的 运动 规划 ， 考 虑 到 机 械 辟 空间 自由 度 的 多 维 性 ， 机 械 辟 本身 及 与 周围 物体 的 碰撞 属性 、 解 空间 的 多 重 性 ， 它 的 规划 会 更 加 复 
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从 本 质 上 来 说 ， 运 动 规划 和 轨迹 规划 没有 实质 的 区 别 ， 它 们 更 多 的 是 应 用 方向 的 不 同 ， 规 划 的 目的 都 是 获得 一 组 具有 时 空 特性 的 轨迹 点 ， 即 时 间 上 递增 的 一 组 离散 空间 坐标 点 。 在 ROs 中 分 别 封装 了 
Move_base 和 Move_Group 两 个 接口 类 ， 前 者 主要 是 针对 无 人 车 的 全 局 路 径 规划 ， 而 后 者 主要 是 面向 机 械 辟 的 运动 规划 。 本 节 主 要 讲述 面向 智能 工业 机 器 人 的 运动 规划 。 


Om ROS 运动 规划 及 其 实现 


6.1 智能 机 器 人 运动 控制 概述 


自主 导航 任务 是 指 智 能 机 器 人 能 够 在 一 个 已 知 或 未 知 的 环境 中 ， 在 无 人 为 操作 的 情况 下 ， 通 过 自身 传感器 感知 周围 环境 确定 自身 位 置 后 ， 以 最 少 的 代价 到 达 既 定 的 区 域 。 在 移动 机 器 人 的 自主 导航 中 ， 
自主 导航 任务 通常 包含 环境 认 知 及 自主 规划 两 方面 的 内 容 。 在 该 过 程 中 ， 运 动 规划 起 着 重要 的 作用 ， 它 确保 移动 机 器 人 能 从 起 点 到 终点 寻找 出 一 条 符合 受制 条 件 且 能 够 被 机 器 人 跟踪 的 路 径 ， 受 制 条 件 通常 


包含 诸如 运动 规律 及 其 他 适用 的 约束 ， 比 如 躲避 障碍 物 、 速 度 约 束 、 平 衡 问题 与 关节 受制 条 件 等 。 


移动 机 器 人 的 自主 规划 类 似 于 人 类 的 寻 路 行为 ， 是 在 已 认 知 的 环境 中 找到 一 条 从 起 点 到 已 知 或 未 知 终点 的 无 碰撞 且 符 合 移动 机 器 人 运动 规律 的 最 优 或 次 优 路 径 。 其 主要 研究 “Where to 
go? ”及 “How to there? ”两 个 问题 ， 即 路 径 规 划 及 基于 已 规划 路 径 的 精确 跟踪 问题 ， 如 图 6-1 所 示 。 
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图 6-1 机 器 人 运动 任务 执行 流程 


运动 规划 方法 分 为 两 个 方面 : 对 于 移动 机 器 人 ， 偏 向 于 意 指 移动 的 路 径 轨 人 迹 规划 ， 如 机 器 人 在 有 地 图 条 件 或 没有 地 图 的 条 件 下 ， 按 什么 样 的 路 径 轨迹 行走 ， 对 于 智能 工业 机 器 人 ， 则 意 指 机 械 臂 末端 行 
走 的 曲线 轨迹 ， 或 是 操作 和 臂 在 运动 过 程 中 的 位 移 、 速 度 和 加 速度 的 曲线 轮廓 。 


为 了 提高 智能 机 器 人 的 操作 效率 ， 且 高 效 完成 特定 的 任务 ， 智 能 机 器 人 必须 以 合理 的 运动 规划 为 前 提 。 运 动 规划 有 离线 和 在 线 两 种 方式 。 传 统 的 运动 规划 多 为 前 者 ， 即 事先 做 好 规划 ， 机 器 人 依 之 运 
动 ， 其 动态 适应 性 较 弱 ， 随 着 嵌入 式 计算 能 力 的 大 幅度 提升 ， 应 用 智能 需求 的 不 断 增强 ， 在 线 运 动 规划 成 为 友 展 方向 。 运 动 规划 可 进一步 细 分 为 路 径 规划 和 轨迹 规划 。 路 径 规 划 的 目标 是 使 路 径 与 障碍 物 的 
距离 尽量 远 ， 同 时 路 径 的 长 度 尽量 短 ; 轨迹 规划 的 目的 主要 是 在 机 器 人 关节 空间 移动 中 使 得 机 器 人 的 运行 时 间 尽 可 能 短 ,或 者 能 量 尽 可 能 小 。 轨 迹 规 划 在 路 径 规划 的 基础 上 加 入 了 时 间 序 列 信息 ， 对 机 器 人 
执行 任务 时 的 速度 与 加 速度 进行 规划 ， 以 满足 光滑 性 和 速度 可 控 性 等 要 求 。 


示 教 再 现 是 传统 路 径 规 划 的 实现 方法 之 一 ， 通 过 操作 空间 进行 示 教 并 记录 示 教 结果 ， 在 工作 过 程 中 加 以 复 现 ， 现 场 示 教 直接 与 机 器 人 需要 完成 的 动作 相对 应 ， 路 径直 观 且 明 确 ， 缺 点 是 需要 经 验 丰富 的 
操作 工人 ， 并 消耗 大 量 的 时 间 ， 路 径 不 一 定 是 最 优化 的 。 为 了 解决 上 述 问 题 ， 可 以 建立 机 器 人 虚拟 模型 ， 通 过 虚拟 的 可 视 化 操作 完成 对 作业 任务 的 路 径 规划 。 


路 径 规划 可 在 关节 活动 空间 中 进行 。 例 如 ， 采 用 5 次 B 样 条 对 机 器 人 的 关节 轨迹 进行 插 补 以 计算 机 器 人 各 个 关节 的 速度 、 加 速度 端点 值 ， 可 根据 平滑 性 要 求 进行 任意 配置 ， 并 将 加 速度 的 平方 相对 于 运动 
时 间 的 积分 作为 目标 遂 数 进行 优化 ， 以 确保 各 个 关节 的 运动 足够 平滑 。 另 外 ， 为 使 关节 空间 的 轨迹 规划 避免 操作 空间 的 奇异 性 问题 ， 可 采用 关节 轨迹 优化 算法 ， 其 利用 6 自由 度 机 器 人 在 任务 过 程 中 某 个 关节 
功能 上 的 元 余 ， 将 机 器 人 奇异 性 和 关节 限制 作为 约束 条 件 ， 采 用 TWA 方 法 进行 优化 计算 。 


这 里 提 到 的 “运动 规划 ” (Motion Planning) 和 “轨迹 规划 (Path Planning) 是 有 区 别 的 ; 一 般 来 说 ，Path Planning 用 于 低 维 的 、 多 运动 体 的 、 全 局 的 路 径 规 划 ， 如 无 人 车 、 无 人 机 领域 。 而 
Motion Planning 主 要 用 于 机 械 臂 、 类 人 机 器 人 领域 ， 如 工业 机 器 人 机 械 劈 的 运动 规划 ， 考 虑 到 机 械 辟 空间 自由 度 的 多 维 性 ， 机 械 辟 本身 及 与 周围 物体 的 碰撞 属性 、 解 空间 的 多 重 性 ， 它 的 规划 会 更 加 复 
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从 本 质 上 来 说， 运动 规划 和 轨迹 规划 没有 实质 的 区 别 ， 它 们 更 多 的 是 应 用 方向 的 不 同 ， 规 划 的 目的 都 是 获得 一 组 具有 时 空 特性 的 轨迹 点 ， 即 时 间 上 递增 的 一 组 离散 空间 坐标 点 。 在 ROs 中 分 别 封装 了 
Move_base 和 Move_Group 两 个 接口 类 ， 前 者 主要 是 针对 无 人 车 的 全 局 路 径 规划 ， 而 后 者 主要 是 面向 机 械 辟 的 运动 规划 。 本 节 主 要 讲述 面向 智能 工业 机 器 人 的 运动 规划 。 


6.2 ROS Movelt 包 介绍 


ROS Movelt 是 专门 为 可 移动 操作 平台 而 开发 的 ROS 软 件 包 ， 它 集成 了 运动 规划 、 三 维 感 知 、 运 动 学 、 运 动 控制 和 导航 领域 的 最 新 成 果 ， 提 供 一 个 开发 高 级 机 器 人 应 用 的 易 用 软件 平台 ， 适 合用 于 进行 


工业 、 商 业 、 科 研 等 领域 机 器 人 新 技术 的 验证 及 集成 化 产品 的 开发 。 


图 6-2 展 示 了 ROS Movelt 的 高 层 架 构 。 在 Movelt 的 架构 中 ，Move_Group 节 点 处 于 核心 地 位 。 此 节点 作为 一 个 “集成 器 ”， 将 各 个 独立 的 构件 组 合 到 一 起 ， 通 过 提供 ROS Action 和 Service 的 方式 供用 


户 使 用 ， 如 图 6-2 所 示 。 


6.2.1 用 户 接口 


用 户 或 应 用 开发 者 可 以 通过 如 下 三 种 方式 使 用 Move_Group 提 供 的 服务 。 


ROS 人 参数 服务 器 


HPH 


MoveGroupAction 


PickAction 
PlaceAction 


Get CartesianPath Service 
Get IK Service 
Get FK Service 


Get Plan Validity Service 
Plan Path Service 


moveit commander 
(Python) 


GUI (Rviz Plugin) 


Execute Path Service 
Get Planning Scene Service 


AttachedObject 
CollisionObject 


PlanningSceneDiff 





图 6-2 ROS Movelt 的 整体 框架 


- 使 用 move_group_intetface 库 提供 的 C++ 编程 接口 。 
- 使 用 moveit_commandet 库 提供 的 Python 编程 接口 。 


- 使 用 rviz 中 的 Motion Planning 播 件 ， 以 GUI 的 方式 使 用 Movelt。 
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Move Group 是 Movelt 提 供 的 主要 的 API 接 口 ，move _group _interface 库 和 moveit commander 库 分 别 支 持 C++ 和 Python。 该 接口 使 得 Movelt 更 容易 在 ROS 中 使 用 。ROS 的 API 主 要 是 通过 
constraints 等 来 配置 一 个 末端 执行 器 的 位 置 约束 ， 或 者 整个 机 器 人 的 关节 约束 。Move_Group 人 允许 用 户 直 接 将 这 些 约束 配置 设置 到 一 个 期 望 的 位 置 、 方 向 ,或 者 进行 整个 机 器 人 的 关节 配置 。 例 如 ， 规 划 机 


器 人 移动 到 目标 位 姿 、 移 动机 器 人 的 某 个 关节 等 。 


rviz 是 ROS 提 供 的 图 形 化 模拟 仿真 环境 ， 运 动 规划 插件 是 rviz 提 供 的 一 个 实时 显示 运动 规划 运行 效果 的 工具 ， 运 动 规划 插件 提供 了 图 形 化 界面 交互 方式 以 方便 Movelt 的 使 用 。 用 户 或 应 用 开发 者 使 用 


C++ 或 Python 的 API 能 够 很 容易 地 通过 rviz 的 运动 规划 插件 向 环境 中 添加 物体 ， 从 而 完成 特定 模拟 仿真 环境 下 的 实验 。 


6.2.2 ROS Movelt 人 参数 配置 
Move Group 作为 一 个 ROS 节 点 ， 它 使 用 ROS 参 数 服务 获取 机 器 人 的 信息 。 所 获取 的 信息 主要 分 为 以 下 三 类 。 
1) 用 XML 表示 的 机 器 人 模型 数据 URDF (统一 机 器 人 描述 格式 ，Unified Robot Description Format) 。URDF 是 为 了 能 够 抽象 描述 一 个 机 器 人 的 硬件 而 创建 的 XML 格式 文件 ， 这 样 就 可 以 让 机 器 人 在 
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模拟 环境 中 运行 ， 让 开发 者 更 快速 地 进行 机 器 人 软件 开 必 和 测试 工作 。 
2) 机 器 人 语义 配置 SRDF (Semantic Robot Description Format) 。SRDF 是 Movelt 的 参数 配置 文件 ， 包 含 机 器 人 的 规划 群 、 位 姿 、 终 端 控制 器 、 虚 拟 关 节 ， 以 及 碰撞 免 测 德 阵 ACM 的 定义 信息 。 


3) Movelt 参 数 配 置 。 包 含 关节 限制 、 运 动 学 参数 、 运 动 规划 参数 等 信息 。 例 如 : 碰撞 免检 和 矩阵 (Avoid Collision Matrix, ACM) 、 虚 拟 关 节 (Virtual Joint) 、 规 划 群 (Planning Group) 的 创 
村 等 运行 Movelt。 


SRDF 也 是 XML 格式 ， 可 配合 URDF 使 用 。 理 论 上 ， 只 要 有 了 SRDF 和 URDF， 就 可 以 完全 定义 一 个 机 器 人 的 Movelt 信 息 。 
建 ， 机 器 人 预 设 位 姿 (Robot Poses) 、 终 端 控制 器 (End Effectors) 、 被 动 关 节 (Passive Joints) 的 配置 ， 等 等 。 完 成 Movelt 的 参数 配置 后 才 可 以 运行 


6.2.3 ROS Movelt 机 器 人 接口 
Move_Group 的 封装 使 得 应 用 开发 者 能 够 直接 利用 它 封 装 好 的 运动 规划 APlI 接 口 ， 可 以 很 容易 地 实现 智能 工业 机 器 人 的 运动 规划 ， 如 图 6-3 所 示 。 


Pre-defined position: 
group.setNamedTarget (“home”); 


group .move () ; 


= my function() ; 


Joint position: 
map<string, double> joints = 
group. setJointValueTarget (Joints); 


qroup .move (); 


Cartesian position: 
Affine3d pose = my function () ; 
group. setPoseTarget (joints) ; 


qroup.move (); 





图 6-3 Move_Group4t API 


清楚 其 实现 细节 。 


通过 调用 上 述 接口 ， 应 用 开发 者 可 以 将 机 械 辟 从 空间 的 一 个 位 姿 移 动 到 另外 一 个 位 姿 ， 但 是 如 何 规划 出 两 点 之 间 的 轨迹 点 的 ， 开 发 者 并 不 
放 运 动 规划 库 ，Open Motion Planning Library) 中 的 特定 规划 算法 RRT， 来 解析 这 一 规划 过 程 。 规 划 的 框架 图 如 图 6-4 所 示 。 
Move_Group 封 装 了 运动 规划 库 ， 同 时 为 上 层 应 用 提供 了 简单 的 调用 接口 。 
Move_Group 的 下 层 调用 了 Planning scene 提 供 的 接口 ， 因 此 Move_Group 是 较 高 层次 的 封装 ， 其 降低 了 调用 的 复杂 度 。 同 时 开发 者 也 可 以 直接 使 用 C+ + 接口 ， 也 就 是 Planning scene 提 供 的 API。 


Move_Group 使 用 ROS topic 和 action 的 方式 与 机 器 人 通信 ， 获 取 机 器 人 的 当前 状态 信息 (如 每 个 关节 的 位 置 ) 、 各 类 传感器 数据 以 及 控制 机 器 人 的 控制 参数 等 。Move_Group 通 过 订阅 /joint_states 主 
题 获 取 机 器 人 当前 的 关节 状态 信息 (如 每 个 关节 的 当前 位 置 ) ，M ovelt 本 身 不 包含 Joint state 的 发 布 程序 ， 该 程序 需要 机 器 人 开发 者 自行 实现 ; Move_Group 通 过 ROS 的 TF 库 获得 坐标 系 变换 数据 ， 包 括 机 


器 人 位 姿 、 其 他 物体 位 姿 等 全 局 性 信息 。 同 样 ， 坐 标 变换 的 发 布 也 需要 开发 者 自行 开发 相应 的 程序 ，Move_Group 通 过 FollowJointTrajectoryAction 接 口 控制 机 器 人 运动 ， 这 是 一 个 ROS Action 接 口 ， 开 发 


者 需要 实现 一 个 相应 的 ActionServer， 用 来 响应 Movelt 的 运动 控制 命令 ， 并 将 命令 转发 给 机 器 人 。 
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图 6-4 规划 框架 图 


6.2.4 ROS Movelt 运 动 规划 实现 


Movelt 通 过 插件 的 形式 使 用 运动 规划 器 构件 ， 这 使 得 Movelt 可 以 灵活 使 用 不 同 的 运动 规划 器 ， 其 默认 运动 规划 器 是 OMPL。 具 体 过 程 是 首先 发 出 运动 规划 请 求 ， 该 请 求 明 确定 义 了 用 户 希 望 运动 规划 
器 完成 的 操作 ， 如 将 机 械 臂 的 每 个 关节 运动 到 一 个 新 位 置 ， 或 者 将 机 器 人 未 端 运 动 到 一 个 新 位 姿 。 运 动 规划 请 求 也 可 以 包含 为 运动 规划 计算 定义 运动 学 约束 ， 如 位 置 、 方 向 、 可 见 性 、 关 节 位 置 等 约束 及 用 
户 自 定义 的 约束 。Move_Group 调 用 运动 规划 器 进行 求解 之 后 ， 生 成 一 个 规划 轨迹 作为 响应 ， 如 图 6-5 所 示 。 若 智能 机 器 人 的 接口 部 分 被 正确 配置 ， 那 么 依据 这 一 轨迹 ， 机 器 人 将 会 移动 到 运动 规划 请 求 所 
指定 的 位 姿 。 
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图 6-5 ”Movelt 的 运动 规划 请 求 与 响应 


6.2 节 介绍 了 简单 的 已 封装 的 Move_Group 接 口 ， 开 发 人 员 也 可 以 直接 调用 Planning interface 提 供 的 C++API 接 口 。 通 过 动态 加 载 插件 的 方式 加 载运 动 规 划 库 ， 主 流 的 运动 规划 库 如 表 6-1 所 示 。 


运动 规划 库 


Descartes 


CLIK 


STOMP 


OMPL/Movelt 





运动 规划 库 中 包含 各 种 运动 规划 算法 的 接口 ， 不 同 的 规划 库 代 表 不 同类 型 的 规划 算法 ， 规 划算 法 主要 分 为 最 优 规划 和 采样 规划 两 类 ， 


特 
全 局 最 优 


了 


路 径 规 划 


局 部 最 优 解 


路 径 规划 适合 大 空间 高 目 由 度 
AE AS TERK 

fal HUG 局 部 最 优 a 
IRIE RIESCI 

5 RI 随机 采样 
阅 单 方便 交互 


多 的 规划 可 行 解 中 找到 最 优 解 ， 而 后 者 是 快速 找到 一 个 有 效 解 。OMPL 是 开源 的 自由 空间 规划 器 ， 也 是 Movelt 上 默认 的 运动 规划 库 。 


OMPL 是 一 个 C++ 运 动 规划 库 ， 其 包含 多 种 运动 规划 的 先进 算法 。 虽 然 OMPL 里 提 到 了 最 优 规划 ， 但 总 体 来 说 它 还 是 一 个 以 采样 规划 为 主体 的 算法 库 。 在 采样 规划 算法 中 ， 
树 (Rapidly-exploring Random Trees, RRT) 和 路 线 图 (Probabilistic Roadmap, PRM) 两 个 算法 ， 当 然 还 有 多 个 其 他 的 运动 规划 算法 ， 如 逐 点 比较 插 补 法 、 数 字 积 分 法 、 


OMPL 的 基本 功能 是 提供 一 个 运动 轨迹 。 给 定 一 个 机 器 人 结构 (假设 有 NN 个 关节 ) ， 给 定 一 个 目标 (比如 终端 移 到 xyz) ， 给 定 一 个 环境 , OMPL 能 提供 一 个 包含 M 个 数组 ， 
最 终 可 以 把 终端 移 到 xyz， 当 然 ， 该 轨迹 应 当 不 会 与 环境 中 的 任何 障碍 发 生 碰撞 。 


参数 集 ， 其 表示 一 个 完整 的 关节 位 置 ， 沿 着 这 一 轨迹 依次 移动 关节 ， 


虽然 已 有 多 个 实现 运动 规划 的 软件 库 和 算法 ， 而 OMPL 由 于 其 模块 化 设计 和 稳定 的 及 时 更 新 ， 成 为 最 流行 的 规划 软件 库 之 一 。 很 多 新 算法 已 在 OMPL 中 得 以 实现 ， 许 多 其 他 软件 (包括 ROS/Movelt) 


可 以 使 用 OMPL 完 成 运动 规划 。 


6.3 ”运动 规划 库 


6.3.1 ”运动 规划 库 概述 


人 工 试 场 法 等 。 


前 者 是 从 众 


经 典 的 算法 是 快速 扩展 随机 


每 一 个 数组 长 度 为 N 的 轨迹 


6.2 节 介绍 了 简单 的 已 封装 的 Move_ Group 接口 ， 开 发 人 员 也 可 以 直接 调用 Planning interface 提 供 的 C+ +API 接 口 。 通 过 动态 加 载 插件 的 方式 加 载运 动 规划 库 ， 主 流 的 运动 规划 库 如 表 6-1 所 示 。 


表 6-1 各 种 运动 规划 库 


运动 规划 库 


Descartes 路 径 规 划 全 局 采样 搜索 


CLIK 路 径 规划 适合 大 空间 高 日 由 度 
ATE AS EK 
局 部 最 优 解 

PR Dal et ae GT 
BEILI EF 

简单 方便 交互 


STOMP 空 国 规划 


OMPL/Movelt 空 国 规划 





不 同 的 运动 规划 库 面向 不 同 的 应 用 领域 ， 各 有 优 缺 点 。 运 动 规划 库 中 包含 各 种 运动 规划 算法 的 接口 ， 不 同 的 规划 库 代 表 不 同类 型 的 规划 算法 ， 规 划算 法 主要 分 为 最 优 规划 和 采样 规划 两 类 ， 前 者 是 从 众 
多 的 规划 可 行 解 中 找到 最 优 解 ， 而 后 者 是 快速 找到 一 个 有 效 解 。OMPL 是 开源 的 自由 空间 规划 器 ， 也 是 Movelt 默 认 的 运动 规划 库 。 


OMPL 是 一 个 C++ 运 动 规 划 库 ， 其 包含 多 种 运动 规划 的 先进 算法 。 虽 然 OMPL 里 提 到 了 最 优 规 划 ， 但 总 体 来 说 它 还 是 一 个 以 采样 规划 为 主体 的 算法 库 。 在 采样 规划 算法 中 ， 经 典 的 算法 是 快速 扩展 随机 
树 (Rapidly-exploring Random Trees, RRT) 和 路 线 图 (Probabilistic Roadmap, PRM) 两 个 算法 ， 当 然 还 有 多 个 其 他 的 运动 规划 算法 ， 如 逐 点 比较 插 补 法 、 数 字 积 分 法 、 人 工 试 场 法 等 。 


OMPL 的 基本 功能 是 提供 一 个 运动 轨迹 。 给 定 一 个 机 器 人 结构 (假设 有 NN 个 关节 ) ， 给 定 一 个 目标 (比如 终端 移 到 xyz) ， 给 定 一 个 环境 , OMPL 能 提供 一 个 包含 M 个 数组 ， 每 一 个 数组 长 度 为 N 的 轨迹 
参数 集 ， 其 表示 一 个 完整 的 关节 位 置 ， 沿 着 这 一 轨迹 依次 移动 关节 ， 最 终 可 以 把 终端 移 到 xyz， 当 然 ， 该 轨迹 应 当 不 会 与 环境 中 的 任何 障碍 发 生 碰撞 。 


虽然 已 有 多 个 实现 运动 规划 的 软件 库 和 算法 ， 而 OMPL 由 于 其 模块 化 设计 和 稳定 的 及 时 更 新 ， 成 为 最 流行 的 规划 软件 库 之 一 。 很 多 新 算法 已 在 OMPL 中 得 以 实现 ， 许 多 其 他 软件 (包括 ROS/Movelt) 
可 以 使 用 OMPL 完 成 运动 规划 。 


6.3.2 ”Movelt 运 动 规划 编程 用 例 


如 前 所 述 ， 在 Movelt 中 ， 规 划 器 是 作为 插件 加 载 设置 的 ， 开 发 者 可 以 使 用 ROS pluginlib interface 加 载 要 使 用 的 任何 规划 器 插件 。 在 加 载 一 个 规划 器 之 前 ， 需 要 确定 和 实例 化 机 器 人 模型 RobotModel 
对 象 和 规划 上 下 文 环境 PlanningScene 对 象 ， 实 例 化 RobotModelLoader 对 象 是 在 ROS 参 数 服务 器 中 查找 所 传递 的 机 器 人 描述 符 参数 ， 然 后 构造 一 个 RobotModel 对 象 ， 代 码 如 下 : 








robot model loader: :RobotModelLoader robot model loader ("robot description"); 


robot model: :RobotModelPtr robot model = robot model loader.getModel () ; 














接着 利用 RobotModel 进 一 步 构 造 一 个 包含 当前 上 下 文 环境 状态 (包括 机 器 人 状态 ) 的 Planningscene 对 象 ， 代 码 如 下 : 


planning scene::PlanningScenePtr planning scene (new planning scene:: PlanningScene ( robot model ) ); 


然后 构造 一 个 插件 加 载 对 象 ， 并 通过 名 称 加 载 到 相应 的 规划 器 ， 代 码 如 下 : 








boost: :scoped ptr<pluginlib: :ClassLoader<planning interface: :PlannerManager>> 
lanner plugin loader; 
anning interface::PlannerManagerPtr planner instance; 


























anner plugin loader.reset (new pluginlib::ClassLoader<planning interface:: PlannerManager>("moveit core", "planning interface::PlannerManager") ) ; 
Lanner instance.reset (planner plugin loader->createUnmanagedInstance (planner plugin name) ); 
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获得 规划 器 管理 接口 对 象 planner_instance， 指 定 规划 的 起 始点 (默认 当前 位 置 ) 和 目标 点 约束 条 件 ， 就 可 以 进行 运动 规划 了 ， 代 码 如 下 : 
planning interface: :MotionPlanRequest req; 

planning interface: :MotionPlanResponse res; 

planning interface::PlanningContextPtr context = planner instance-> getPlanningContext( planning scene, req, res.error code ); 
context->solve (res); 

if (res.error code .val != res.error code_.SUCCESS) { ROS ERROR("Could not compute plan successfully") ; } 




















通过 查看 对 象 MotionPlanResponse 中 的 状态 位 error_code_.val 可 以 判断 规划 是 否 成 功 ， 同 时 将 规划 好 的 路 径 保存 在 req 对 象 中 ， 之 后 就 可 以 借助 ROS 工 具 进 行路 径 可 视 化 。 


6.3.3 规划 接口 定义 


规划 接口 (Planning interface) 为 Movelt 的 运动 规划 库 的 加 载 和 实现 提供 了 一 个 统一 的 接 入 接口 。 


Planning interface 的 命名 空间 中 主要 定义 了 PlannerManager 和 PlanningContext 两 个 接口 类 ， 如 图 6-6 所 示 。 


Planning interface::PlannerManager Planning interface::PlannerContext 


® setPlanningContext () % solve () 


图 6-6 PlannerManager4ePlanningContext Km Rh KAA 


从 图 6-6 中 可 以 看 出 PlannerManager 定 义 了 接口 函数 : 





virtual PlanningContextPtr getPlanningContext (const planning scene::Planning-SceneConstPtré& planning scene, const MotionPlanRequest &req, moveit _msgs::MoveltErrorCodes &error cı 





它 是 一 个 纯 虚 国 数 ， 接 收 规划 场景 和 规划 请 求 (包括 起 始 位 置 和 终止 位 置 ) 两 个 参数 ， 返 回 PlanningContext 对 象 的 SharePtr 智 能 指针 。 


PlanningContext 定 义 了 以 下 接口 : 














/** solve (MotionPlanResponse &res) 求 解 运动 规划 问题 ， 并 将 结果 存储 在 res 中 ， 这 个 函数 中 不 会 清除 用 于 计算 的 结构 化 数据 ， 直 到 调用 析 构 函数 或 者 clear () 函数 才 会 清除 数据 */ 
virtual bool solve (MotionPlanResponse &res) = 0; 
/** solve (MotionPlanDetailedResponse &res) 求 解 运 动 规划 问题 ， 并 将 详细 结果 存储 在 res 中 ， 这 个 函数 中 不 会 清除 用 于 计算 的 结构 化 数据 ， 直 到 调用 析 构 函数 或 者 clear () 函数 才 会 清除 数据 */ 


virtual bool solve (MotionPlanDetailedResponse &res) = 0; 















































在 上 一 步调 用 中 返回 了 PlanningContext 对 象 指针 ， 接 着 调用 PlanningContext 的 成 员 函 数 solve () 。 


因此 ， 要 使 运动 规划 库 能 够 加 入 Movelt 平 台中 ， 就 必须 实现 PlannerManager 和 PlanningContext 两 个 接口 类 及 其 中 的 接口 函数 。 接 下 来 来 看 一 下 OMPL 是 如 何 实现 这 两 个 接口 类 的 。 


6.3.4 OMPL 接 口 分 析 


OMPL 接 口 类 位 于 Moveit_planners 包 中 ， 如 图 6-7 所 示 。 


Planning interface::PlannerManager Planning interface::PlanningContext 


get Planning Context () *sglve () 


ompl interface::OMPLPlannerManager 
急 ompl interface :boost::scoped_ptr<OMPLInterface> 


SeetPlanningContext() | 


OMPL Interface 
Scontext_manager_: omplVinterface::PlanningContextManager 


¢ getPlanningContext () 






ompl interface::ModelBasedPlanningContext 
Sompl simple setup : :ompl::geometric::SimpleSetupPtr 


ompl interface::PlanningContextManager 
&ompl parallel plan : ompl::tools::ParallelPlan 
@ getPlanningContext () 








ompl::geometric::Simple Setup omplI::tools::ParallelPlan 
& planner_:base::PlannerPtr & planners_:: vector<base::PlannerPtr> 


ompl::base::base::Planner 


TET) 
®solve () 


图 6-7 OMPL 接 口 


由 图 6-7 所 示 的 类 关系 图 可 以 看 出 Planning interface 是 如 何 实现 OMPL 第 三 方 库 与 Movelt 的 接口 的 。 


由 图 6-7 可 以 看 到 ， 规 划 器 管理 接口 类 主要 有 两 个 ， 一 个 是 Simplesetup， 实 现 了 单线 程 的 运动 规划 ; 另 一 个 是 ParallelPlan， 可 以 实现 并 行 的 运动 规划 。 如 果 solve 中 的 参数 运行 次 数 count 大 于 1， 则 
调用 并 行规 划 的 接口 ， 否 则 就 调用 单线 程 的 规划 接口 。 多 线程 并 行规 划 ， 可 以 从 规划 出 来 的 几 个 路 径 中 选 出 最 优 的 那个 。ParallelPlan 接 口 的 实现 算法 如 图 6-8 所 示 。 


由 图 6-8 可 以 看 出 ， 具 体 的 规划 算法 类 分 为 很 多 种 ， 最 著名 的 就 是 RRT 和 PRM 算 法， 图 6-8 只 是 列 出 了 其 中 的 几 种 规划 类 ， 上 有 具体 可 以 查看 OMPL 开 源 库 。 
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图 6-8 ”ParallelPlan 接 口 的 实现 算法 


6.4 RRT 算 法 


在 采样 规划 算法 中 ， 快 速 扩展 随机 树 (Rapidly-exploring Random Trees, RRT) 尤为 经 典 。 基 于 快速 扩展 随机 树 的 路 径 规 划算 法 ， 通 过 对 状态 空间 中 的 采样 点 进行 碰撞 检测 ， 从 而 避免 了 对 空间 的 建 
模 ， 由 此 能 够 有 效 解 决 高 维 空 间 和 复杂 约束 的 路 径 规划 问题 。 其 特点 是 能 够 快速 有 效 地 搜索 高 维 空间 ， 通 过 状态 空间 的 随机 采样 点 ， 把 搜索 导向 空白 区 域 ， 从 而 寻找 到 一 条 从 起 始点 到 目标 点 的 规划 路 径 ， 
适合 解决 多 自由 度 机 器 人 在 复杂 环境 中 和 动态 环境 中 的 路 径 规 划 。RRT 算 法 是 利用 随机 采样 生成 扩展 随机 树 的 ， 从 而 对 同一 任务 重复 规划 会 产生 不 同 的 路 径 。 


6.4.1 基本 RRT 算 法 描述 


RRT 是 多 维 空间 中 的 一 种 很 有 效率 的 规划 算法 。 它 以 一 个 初始 点 作为 根 节点 ， 通 过 随机 采样 增加 叶子 节点 的 方式 ， 生 成 一 个 随机 扩展 树 ， 当 随机 树 中 的 叶子 节点 包含 了 目标 点 或 进入 了 目标 区 域 ， 便 可 
以 在 随机 树 中 找到 一 条 从 初始 点 到 目标 点 的 路 径 。 基 本 RRT 算 法 如 下 所 示 : 


Function RRTPlan; BOOL(env: environment, T; RRTTree, goa; node) 
Var Guts cutis Gort Bode 

While (search time/space remaining) do 

Gumi Choose largel (4.54) 

Geen Nearest (Ts Gage) 

TE (DiS Canes (Gumede Goi) SUisetanceInreshoid) Chen 
Return true 

Greg LENG. (Guemes ergot) 

If (QpewANULL) then 

T.AddNode (Gnpew) 


Return false 


— 
SO OC N GO OP U MM H 


Function CheoseTarget (ds node); node 


11 Var p: real 

12 p=Random (0, 1.0) 
la if O<p<GoalProb then 

14 Return goal 

15 Else if GoalProb<p<1.0 then 
16 Return RandomNode():; 


9 





在 随机 树 的 生长 过 程 中 (Function RRTPlan 的 第 1 ~ 10 行 ) ， 初 始 传 入 的 随机 树 T 只 包含 一 个 节点 : 根 节点 qinit。 首 先 ChooseTarget 国 数 从 状态 空间 中 随机 选择 一 个 采样 点 qtarget (8347) ; 然 
后 ，Nearest 函 数 从 随机 树 中 选择 一 个 距离 qtarget 最 近 的 节点 qnearest (第 4 行 ) ; 最 后 ，Extend 函 数 通 过 从 qnearest 向 qTarget 扩 展 一 段 距离 s (为 RRT 生 长 的 最 小 单位 ， 称 为 步 长 ) ， 得 到 一 个 新 的 节 
点 qnew (第 7 行 ) 。 如 果 qnew 与 障碍 物 发 生 磁 撞 ， 则 Extend 水 数 返 回 空 ， 放 弃 这 次 生长 ， 否 则 将 qnew 加 入 到 随机 树 中 。 重 复 上 述 步骤 直到 qnearest 和 目标 点 qgoal 距 离 小 于 一 个 阅 值 
DistanceThreshold， 此 时 代表 随机 树 到 达 了 目标 点 ， 算 法 返回 成 功 (第 5 ~ 6 行 ) 。 为 了 使 算法 可 控 ， 代 码 中 加 入 了 运行 时 间 上 限 或 搜索 的 节点 总 数 上 限 (第 2 行 ) 。 如 果 在 限制 期 间 内 无 法 到 达 目 标点 ， 则 
算法 返回 失败 ，RRT 算 法 示意 如 图 6-9 所 示 。 
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图 6-9 RRT HAR “ 意 
为 了 加 快 随机 树 到 达 目 标点 的 速度 ， 改 进 方法 是 : 在 随机 树 每 次 的 生长 过 程 中 ， 根 据 随机 概率 来 决定 qtarget 是 目标 点 还 是 随机 点 。 在 ChooseTarget 函 数 (第 11~16 行 中 设 定 参 数 GoalProb， 


到 一 个 0~ 1.0 的 随机 值 p， 当 0<p<GoalProb 的 时 候 ， 随 机 树 朝 目标 点 生长 (第 13~14 行 ) ; 当 GoalProb<p<1.0 (第 15~16 行 ) 时 ， 随 机 树 朝 一 个 随机 方向 生长 。 


RRT 算 法 的 执行 流程 如 图 6-10 所 示 。 
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图 6-10 ”RRT 算 法 的 执行 流程 


RRT 算 法 的 执行 过 程 演示 如 图 6-11 所 示 。 


每 次 





图 6-11 RRT 算 法 执行 过 程 演 示 


6.4.2 ”RRT 算 法 的 性 能 分 析 
RRT 算 法 的 独特 优点 在 于 其 在 考虑 微分 约束 而 易 产生 多 维度 的 情况 下 ， 不 会 出 现 基于 几何 构造 等 常规 算法 易 出 现 的 维度 灾难 问题 ， 因 此 可 以 直接 应 用 于 非 完整 运动 规划 及 多 自由 度 机 器 人 的 运动 规划 
中 ; 采用 RRT 算 法 规划 所 得 出 的 是 由 许多 段 曲线 组 成 的 轨迹 ， 其 符合 移动 机 器 人 运动 过 程 应 具备 的 非 完整 微分 约束 。 但 其 仍 有 一 些 需要 进一步 改进 与 优化 之 处 ， 具 体 分 析 如 下 。 


1) RRT 采 用 最 近邻 原则 选择 最 近邻 节点 和 新 节点 ， 这 会 使 规划 器 在 选择 节点 时 直接 偏向 最 邻近 的 随机 目标 点 的 节点 ， 偏 移 目标 随机 点 方向 上 的 节点 会 被 舍弃 ， 这 样 就 会 使 随机 树 在 局 部 区 域 难以 向 侧面 
或 随机 树 后 方 扩展 ， 除 非 等 到 特征 区 域 出 现 特定 的 随机 目标 点 ， 将 随机 树 引 向 该 区 域 ， 而 出 现 这 种 情况 的 概率 比较 小 ， 需 要 花费 很 长 时 间 。 


2) 由 于 RRT 算 法 随机 搜索 的 本 质 特征 ， 算 法 搜索 具有 盲目 性 ， 规 划 路 径 的 代价 比较 大 。 


3) RRT 算 法 的 另外 一 个 缺陷 是 “缺少 记忆 ”， 如 果 RRT 在 扩展 过 程 中 碰 到 潜在 边缘 出 现 碰撞 扩展 失败 的 情况 时 ， 在 后 续 的 扩展 过 程 中 ， 再 碰 到 同一 区 域 随 机 目标 点 时 ， 会 重复 党 试 扩展 已 经 扩展 的 节 
点 ， 或 者 处 于 碰撞 区 域 无 法 实例 化 的 节点 。 许 多 状态 点 可 能 会 被 重复 多 次 选择 并 扩展 ， 尽 管 它们 在 之 前 的 扩展 迭代 中 已 经 由 于 碰撞 检测 不 能 扩展 ， 或 者 已 经 有 了 子 节点 ， 但 是 依然 满足 代价 方程 ， 所 以 又 会 
被 再 次 选 定 ， 对 于 这 些 节点 的 每 一 次 重复 选取 都 需要 尝试 碰撞 检测 及 其 他 运算 ， 从 而 影响 规划 的 效率 。 


4) RRT 算 法 采用 简单 的 度量 函数 来 反映 状态 点 到 随机 目标 点 的 距离 ， 以 及 在 选择 最 佳 输入 时 也 同样 采用 简单 的 欧 几 里 得 直线 距离 度量 消 数 。 这 种 简单 的 机 制 无 法 很 好 地 适用 于 在 复杂 环境 中 运行 的 并 且 
要 考虑 微分 约束 的 判断 ， 但 是 在 局 部 小 规模 的 情况 下 ， 在 选择 节点 或 输入 时 ， 由 于 没有 考虑 周围 障碍 物 的 约束 ， 简 单 的 最 近 距 离 选 择机 制 在 没有 考虑 周围 环境 约束 的 状态 下 无 法 选 出 一 个 理想 的 节点 ， 以 既 
能 保证 最 近邻 原则 ， 又 可 以 保证 选 定 的 节点 趋 于 碰撞 最 小 化 。 


6.4.3 ”RRT 算 法 的 几 种 优化 


基本 RRT 算 法 存在 动态 环境 中 规划 路 径 不 稳定 的 问题 。 由 于 随机 树 在 自由 空间 中 随机 地 选择 生长 方向 ， 且 环境 中 的 障碍 物 是 运动 的 ， 从 而 导致 了 每 个 周期 里 随机 树 的 生长 差别 很 大 ， 所 以 生成 的 路 径 不 
稳定 。ERRT、DRRT、MP-RRT、CRRT、RRT-Connect 等 RRT 优 化 算法 在 一 定 程度 上 改进 了 这 一 问题 。 另 外 还 有 目标 偏好 RRT (Goal-Biasing-RRT) 算法 与 双向 RRT (bi-RRT) 算法 被 相继 提出 并 应 用 于 实 
战 中 ， 这 两 种 改进 的 RRT 算 法 都 是 基于 将 规划 树 向 终点 区 域 牵引 的 思想 ， 在 规划 过 程 中 既 有 RRT 算 法 的 随机 性 质 ， 也 伴 有 向 着 终点 扩展 的 特性 。 


ERRT 算 法 中 提出 了 路 径 节点 集合 (Way Point Cache) 的 概念 ， 将 一 个 周期 的 路 径 节点 人 存 入 该 集合 中 ， 下 一 周期 将 按照 一 定 的 比例 从 路 径 节点 集合 中 随机 选择 节点 作为 Choose-Target 函 数 的 返回 值 。 


DRRT 和 MP-RRT 都 是 通过 对 上 周期 随机 树 与 新 环境 进行 碰撞 检测 ， 保 留 可 用 的 子 树 ， 然 后 在 其 基础 上 进行 重新 规划 。 不 同 之 处 在 于 ，DRRT 只 保留 含有 根 节点 的 子 树 ， 在 其 基础 上 扩展 到 目标 点 ; 而 
MP-RRT 保 留 了 全 部 的 可 用 子 树 ， 并 从 中 随机 选择 进行 连接 ， 生 成 新 的 路 径 。 


基本 RRT 算 法 生成 的 路 径 具 有 随机 性 ， 而 改进 的 RRT 算 法 通过 历史 信息 使 新 规划 出 的 路 径 在 一 定 程度 上 保持 稳定 。 结 合 以 上 两 种 方法 的 特点 ， 每 周期 规划 出 两 条 路 径 : 一 条 稳定 路 径 和 一 条 随机 路 径 。 稳 
定 路 径 是 通过 对 历史 路 径 进行 剪 枝 和 重新 规划 ， 使 其 尽 可 能 接近 上 周期 的 路 径 ; 随机 路 径 是 由 基本 的 RRT 方 法 得 出 的 。 然 后 对 比 两 条 路 径 ， 得 到 一 条 较 优 的 路 径 。 该 算法 可 以 防止 路 径 陷 入 不 优 的 稳定 状 
态 ， 并 使 之 逐渐 远近 较 优 路 径 ， 该 算法 全 称 为 Compare RRT 算 法 ， 简 写 为 CRRT。 


目标 偏好 RRT 算 法 是 对 随机 树 在 状态 空间 均匀 地 获取 目标 点 进行 改进 所 获得 的 新 算法 。 该 算法 在 大 部 分 扩展 过 程 中 随机 采取 目标 点 qtarget， 而 以 一 定 的 小 概率 选取 目标 区 域 Cgoal 中 的 点 来 取代 目标 
点 ， 从 而 将 随机 树 扩展 方向 引 向 目标 区 域 扩展 。 


RRT-Connect 算 法 是 RRT 算 法 的 另外 一 个 扩展 版 本 ， 该 算法 一 旦 选 定 一 个 扩展 方向 ， 就 会 尽 可 能 多 地 向 前 扩展 ， 直 到 在 前 方 遇 到 障碍 物 ;， 这 种 贪 梦 式 的 算法 在 非 结构 化 环境 及 开阔 环境 中 可 以 通过 一 次 
迭代 到 达 边 绿 区 域 。 


bi-RRT 算 法 是 基于 RRT-Connect 算 法 的 思想 提出 来 的 ， 它 也 是 基于 向 目标 终点 牵引 式 的 思想 ， 是 比 目 标 偏好 RRT 单 树 算法 效率 更 高 、 应 用 更 加 普遍 的 RRT 扩 展 算法 。 它 采取 从 起 点 与 终点 分 别 扩展 出 随 
机 树 并 相向 扩展 的 方式 ， 在 选取 随机 节点 后 ， 首 先 扩展 其 中 的 一 棵 树 ， 另 外 的 一 棵 树 以 新 产生 的 节点 作为 随机 目标 进行 下 一 步 扩展 ， 并 在 下 一 次 迭代 过 程 中 更 换 两 棵 树 的 扩展 顺序 。 


6.4.4 CRRT 算 法 


在 此 主要 介绍 RRT 的 优化 算法 一 一 CRRT 算 法 ，CRRT 主 算法 描述 如 下 : 首先 通过 RRT 算 法 进行 路 径 规划 ， 得 到 随机 扩展 树 Tnew (第 2~ 3 行 ) 。T 为 上 周期 的 随机 树 ， 如 果 T 为 空 或 本 周期 机 器 人 的 目标 点 
与 上 周期 发 生 了 较 大 的 变化 ， 则 直接 返回 Tnew (第 4~ 5 行 ) ; AM, WHT ALR (第 7 行 ) ， 将 修正 后 的 T 与 Tnew 进 行 比较 ， 返 回 一 条 较 优 路 径 (第 8 行 ) 。CRRT 算 法 如 下 所 示 : 





Function CRRTPathPlan: RRTTree 


(env: environment, T: RRTTree, dint: node, dra: node) 


— 


VAr Tnew' CrtTree 
Tanew .add ( [init ) 
RRTPlan (env, Thasics goat) 


Lat D9 


A Tf Empty (T) or NotSimilarGoal()then 
5 Return Phew 

6 Else 

7 TrimAndReplan (env, T, Ginit, goal) 

8 Return Cheoselree (Ts Tier) 


Function TrimAndReplan 


(env: environment T: RRTTree, Gini: node, Ceo: node) 


9 Var F: RRTForest 

10 Var q: node 

Ta. For each node q & T, F do 
12 If not NodeValid (aq) 

13 DeleteNode (q) 

A For each SubTree st EF 

a IË not st.root © WayPoints(T) 
16 DeleteTree (st) 

17 T: TAE (a) 

18 For each SubTree st E&F 
19 RRTPlan (env, T, Root (st)) 
20 RRTPlan (env, T, goat) 


Hh, STENT SRS AAS A DTrimAndReplan (第 9~ 2047) : 首先 ， 将 T 中 与 动态 障碍 物 发 生 碰撞 的 节点 删除 (第 11 ~ 1347) 。T 被 分 成 若干 个 子 树 ， 形 成 森林 F。 在 F 中 ， 将 根 节点 不 
属于 上 周期 路 径 节 点 的 子 树 删除 (第 14 ~ 16 行 ) 。 按 照 从 初始 点 到 目标 点 的 顺序 ，F 中 的 子 树 排列 依次 为 T1，T2，…，Tn。 然 后 ， 重 新 构造 T， 以 本 周期 的 机 器 人 状态 qinit 作 为 T 的 根 节 点 。 使 用 基本 RRT 算 
法 ， 依 次 将 T 生 发 到 T1，T2，…，Tn 的 根 节点 ， 将 子 树 连接 起 来 。 最 后 ， 将 T 生 发 到 目标 节点 qgoal。 


CRRT 算 法 第 8 行 是 TH 和 Tnew 进 行 对比 的 函数 ， 其 过 程 分 为 两 步 : 先生 成 两 条 路 径 ， 再 通过 对 比 路 径 长 度 得 到 较 优 解 。 


路 径 生 成 方法 为 : 从 qgoal 开 始 ， 一 直 追 溯 其 父 杀 节点 ， 直 到 随机 树 的 根 节点 qinit， 形 成 路 径 节 点 队列 (qo0，91，.…，9N) 。 其 中 ，q0 为 初始 点 qinit，qN 为 目标 点 qgoal。 然 后 ， 对 路 径 进行 平滑 处 理 ， 
采用 贪心 思想 ， 令 qtemp 为 初始 点 q90， 依 次 尝试 连接 q1，q2，…，QqN 直 到 无 法 直接 到 达 的 第 一 个 节点 qij， 则 qi-1 为 从 qtemp 开 始 的 可 到 达 点 ， 用 一 条 直线 取代 qtemp 和 qi-1 之 间 的 路 径 。 令 qtemp 等 于 qi-1， 重 
复 上 述 的 过 程 ， 直 到 qtemp 等 于 qN 为 止 。 最 终 ，q0，9q1，…，9qN 直 接 的 路 径 被 平滑 处 理 为 若干 相连 的 直线 。 路 径 平滑 算法 如 下 所 示 : 


Function Smooth (T: RRTTree): Path 
Var P, P: Path 
Ttemp: node 
) =Get Pah (T) 
temp = Clo 
P' .AddNode (qo) 
While Ctemp!=Gn do 


For Gach: node Gi Gimp o s Ga de 


If not Connect (ctem, qi) then 


temp = Gi-1 
P" .AddNode ( Gtemp } 
Break 

P' .AddNode (ay) 


Return 了 





T 和 Tnew 经 过 平滑 处 理 后 ， 可 以 得 到 两 条 路 径 P 和 Pnew。 比 较 P 和 Pnew 的 长 度 ， 如 果 Pnew 长 度 与 P 长 度 的 比值 小 于 预 设 的 阅 值 Threshold， 则 返回 T， 保 持 路 径 的 稳定 ; 人 否则， 返回 Tnew， 表 示 随 机 路 径 要 
优 于 稳定 路 径 ， 此 处 使 用 新 路 径 ， 从 而 保证 了 路 径 向 最 优 解 逼近 。 


6.5 ”本章 小 结 


本 章 主要 介绍 了 ROS 运 动 规划 及 其 实现 方法 ， 从 运动 规划 和 轨迹 规划 两 方面 说 明 其 在 智能 机 器 人 控制 中 的 重要 意义 。 对 于 运动 规划 与 轨迹 规划 ， 其 本 质 都 是 获得 一 组 具有 时 空 特性 的 轨迹 点 ， 以 确定 机 
器 人 的 运动 目标 。 为 实现 运动 规划 ，ROS 提 供 的 Movelt 包 封装 了 运动 规划 、 三 维 感知 、 运 动 学 、 运 动 控 制 和 导航 等 功能 并 提供 了 开发 接口 ， 开 发 者 可 以 通过 配置 相应 的 运动 规划 库 实现 对 机 器 人 控制 程序 的 
开发 调试 。OMPL 是 典型 的 运动 规划 库 ， 提 供 了 常用 的 运动 规划 算法 的 实现 ， 可 配合 Movelt 使 用 ， 本 章 以 编程 用 例 的 方式 介绍 了 OMPL 的 常用 接口 ， 以 供 读者 参考 。 另 外 ， 本 章 还 介绍 了 最 常用 的 采样 规划 
算法 一 一 RRT， 从 原理 、 性 能 分 析 和 优化 三 个 方面 对 RRT 算 法 进行 了 详细 的 介绍 。 





第 7 草 ”基于 ROS 的 智能 机 器 人 系统 开 友 方法 


7.1 ROS 实 时 化 


ROS 最 初 是 为 科研 而 设计 的 ， 虽然 功能 丰富 ， 但 缺乏 实时 性 。 对 于 工业 、 军 用 等 领域 ,， 实 时 性 属于 基本 要 求 ， 所 以 为 ROS 增 强 实时 性 非常 有 必要 。 新 发 布 的 ROS2.0 已 经 对 实时 性 进行 了 改善 。 针 对 ROS 
的 实时 性 ， 目 前 有 两 种 改进 方法 ， 分 别 为 混合 实时 ROS 体 系 结构 RGMP-ROS 和 和 结合 OROCOS 实 现实 时 性 。 


第 7 草 ”基于 ROS 的 智能 机 器 人 系统 开 友 方法 


7.1 ROS 实 时 化 


ROS 最 初 是 为 科研 而 设计 的 ， 虽然 功能 丰富 ， 但 缺乏 实时 性 。 对 于 工业 、 军 用 等 领域 ,实时 性 属于 基本 要 求 ， 所 以 为 ROS 增 强 实时 性 非常 有 必要 。 新 发 布 的 ROS2.0 已 经 对 实时 性 进行 了 改善 。 针 对 ROS 


的 实时 性 ， 目 前 有 两 种 改进 方法 ， 分 别 为 混合 实时 ROS 体 系 结构 RGMP-ROS 和 结合 OROCOS 实 现实 时 性 。 


7.1.1 ”混合 实时 ROS 体 系 结构 RGMP-ROS 
将 ROS 运 行 在 多 核 处 理 器 上 ， 可 以 形成 一 个 混合 实时 ROS 体 系 结构 RGMP-ROS， 它 由 两 部 分 组 成 : 一 部 分 是 实时 子 系统 RTOS， 另 一 部 分 是 非 实时 子 系统 GPOS。 它 们 分 别 占 用 不 同 的 CPU ， 通 过 共享 
内 存 实现 通信 。 其 中 GPOS 由 运行 在 非 实 时 操作 系统 内 核 上 的 ROS 节 点 组 成 ，RTOS 由 运行 在 实时 操作 系统 内 核 上 的 实时 ROS 节 点 组 成 。 


RGMP-ROS 系 统 体系 结构 如 图 7-1 所 示 。 在 应 用 层 ，ROS 节 点 被 分 为 实时 和 非 实时 节点 ，ROS 节 点 之 间 的 通信 分 为 核 内 通信 和 核 间 通信 ， 核 间 通 信 通 过 共享 内 存 实 现 。 在 操作 系统 内 核 层 中 ，GPOS 和 
RTOS 分 别 驻 留 在 不 同 的 硬件 资源 上 ， 包 括 处 理 器 核 、 中 断 和 相关 的 硬件 资源 。 在 硬件 层 ， 硬 件 资源 也 是 相互 分 离 的 ， 从 而 保证 非 实 时 ROS 节 点 和 实时 ROS 节 点 可 以 分 别 运 行 在 GPOS 和 RTOS 上 。 





通信 


任务 /机 抽 
GPOS 内 核 RTOS 内 核 


非 实时 设备 和 实时 设备 和 
其 他 硬件 资源 | + | 其 他 硬件 资源 





图 7-1 RGMP-ROS 系 统 体系 结构 
RGMP-ROS 是 由 ROS、Nuttx 和 RGMP 发 展 而 来 的 ， 下 面 对 它 们 分 别 进行 简单 介绍 。 


(1) ROS 


图 7-2 展 示 了 ROS 的 通信 层 模 型 和 它 对 应 的 OSI 模 型 。 可 以 看 出 ， 应 用 层 协 议 是 基于 构建 在 TCP/IP 架 构 上 的 XML-RPC HTTP 协 议 实现 的 ， 因 此 ， 节 点 间 的 消息 不 是 通过 数据 包 的 形式 传递 ， 而 是 通过 满 
足 HTTP 协 议 的 网 页 文件 进行 传递 的 。 


OST RAY 
应 用 层 





数据 链接 层 





图 7-2 ”ROS 通信 模型 及 其 对 应 的 OSI 模 型 
(2) Nuttx 
Nuttx 是 一 个 开源 的 实时 操作 系统 内 核 ， 易 于 扩展 ROS。Nuttx 对 内 存 的 需求 非常 小 ， 只 需要 很 小 的 内 存 就 可 以 满足 不 同 的 应 用 案例 。 Nuttx 的 另 一 个 优点 是 易于 移植 和 扩展 ， 可 以 方便 地 移植 到 新 的 处 
(3) RGMP 
RGMP 是 一 个 软件 集成 框架 ， 如 图 7-3 所 示 。 它 可 以 用 来 在 一 个 控制 器 有 多 核 处 理 器 的 情况 下 同时 运行 两 个 操作 系统 内 核 ， 一 个 是 基于 Linux 的 一 般 用 途 的 操作 系统 内 核 ， 另 一 个 是 实时 操作 系统 内 核 。 


GPOS 和 RTOS 分 别 占 用 了 不 同 的 CPU， 它 们 之 间 通 过 VNET 通 道 通信 。VNET 是 一 个 虚拟 的 网 络 接口 ， 通 过 共享 内 存 实 现 数据 交换 。 同 时 ， 使 用 基于 TCP/IP 协 议 的 标准 网 络 接口 协议 也 可 以 建立 实时 系统 
Nuttx 和 Linux 之 间 的 标准 网 络 通信 接口 。 
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图 7-3 RGMP 软 件 集成 框架 


RGMP 有 三 个 功能 。 第 一 个 功能 是 引导 整个 系统 ， 当 RGMP 启 动 时 ，Linux 首 先 初始 化 包括 CPU、 内 存 、 外 围 设 备 在 内 的 所 有 硬件 资源 。 然 后 ，GPOS 模 块 释放 两 块 内 存 和 一 个 CPU。 之 后 ，RTOS 就 会 


能 是 提供 丰富 的 编程 资源 ， 如 中 断 注册 、 内 存 


实现 运行 过 程 中 GPOS 和 RTOS 之 间 的 通信 ， 通 信 通 过 释放 的 男 一 块 内 存 块 来 实现 。 第 三 个 功 
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BGZE 


运行 在 释放 的 CPU 上 。 第 二 个 功 


被 装载 在 其 中 一 块 内 存 上 ， 并 
分 配 、BIOS 读 取 等 接口 函数 。 这 样 ， 用 户 就 可 以 快速 地 开发 自己 的 机 器 人 混合 操作 系统 。 


7.1.2 案例 分 析 
17 ， 该 案例 通过 软件 测 


行 RKTOS。 使 用 运行 


GPOS， 在 另 一 个 CPU 上 运行 在 双核 处 理 器 上 的 RGMP-ROS 来 控制 六 自由 度 的 模块 化 机 械 辟 


/— 


1. 总 体 设计 
RGMP-ROS， 即 在 一 个 CPU 上 运行 


4—= 


1T 
在 Nuttx 上 的 实时 ROS 系 统 。 每 一 部 分 都 有 自己 的 CPU、 内 存 、 中 


本 案例 将 实现 在 双核 处 理 器 上 运 

试 和 实验 证 明了 它 的 有 效 性 和 效率 。 
该 案例 实现 的 双核 处 理 器 上 的 RGMP-ROS 由 两 部 分 组 成 ， 如 图 7-4 所 示 。 一 部 分 是 安装 在 Linux 上 的 非 实 时 ROS 系 统 ， 另 一 部 分 是 运行 

断 和 外 围 设备 。 这 两 部 分 相应 地 形成 实时 节点 和 非 实 时 节点 两 种 类 型 的 ROS 节 点 。 节 点 之 间 的 通信 和 是 通过 TCP/IP 协 议 上 的 XML-RPC 调 用 来 实现 (EE) 的 。 因 为 实时 ROS 和 非 实 时 ROS 运 行 在 不 同 的 CPU 
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运行 在 同一 台 机 器 上 ， 所 以 可 以 通过 共享 内 存 来 实现 通信 ) 。 


上 ， 因 此 可 以 使 用 RGMP_VENT 作 为 通信 通道 (底层 数据 通信 可 以 通过 以 太 网 或 VNET 来 实现 ， 由 于 它们 
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图 7-4 RGMP-ROS 体 系 架构 


2. 具 体 实现 
基于 |A32SMP Intel 的 双核 处 理 器 可 用 于 实现 RGMP-ROS， 如 图 7-5 所 示 。Intel 的 双核 处 理 器 拥有 强大 的 计算 能 力 和 对 复杂 软件 的 支持 能 
(1) 中 断 处 理 
硬件 隔离 是 通过 |A32SMP 体 系 结构 的 APIC (高 级 可 编程 中 断 控 制 器 ) 体系 结构 来 实现 的 。 采 用 APIC 体 系 结构 ， 每 一 个 CPU 都 会 用 一 个 本 地 APIC (LAPIC) 来 接收 来 自 /O APIC 产 生 的 中 断 消息 ,I/O 
每 一 个 本 地 APIC 都 拥有 一 个 独一无二 的 ID， 这 样 程序 员 就 可 以 编程 实现 让 IMO APIC 向 一 个 特定 的 CPU 交付 一 个 特定 的 中 断 ， 从 而 实现 向 一 个 特定 的 操作 系统 


APIC 会 收集 并 且 管 理 来 自 /O 设 备 的 中 断 信号 。 
交付 特定 的 中 断 。 通 过 这 种 机 制 可 以 实现 一 个 硬件 分 配器 ， 为 不 同 的 操作 系统 分 配 不 同 的 中 断 。 
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图 7-5 在 双核 处 理 器 上 实现 RGMP-ROS 


(2) 引导 两 个 操作 系统 内 核 


采用 CPU 热 插 拔 的 方式 可 实现 在 两 个 CPU 上 运行 两 个 操作 系统 内 核 。CPU 热 播 拔 是 Linux 内 核 的 一 个 特征 ， 可 以 用 来 动态 地 使 能 或 茶 用 一 个 CPU， 有 具体 通过 调用 两 个 内 核 API 函 数 cpu_up () 和 
cup_down () 来 实现 。 当 使 用 一 个 CPU 编号 作为 参数 调用 cpu_down () 时 ， 这 个 CPU 上 的 进程 和 中 断 就 会 迁移 到 其 他 CPU 上 ， 并 且 这 个 CPU 会 一 直 休 了 眠 ， 直 到 cpu_up () 被 调用 为 止 。 


IA32SMP 体 系 结构 中 的 进程 间 中 断 (Inter-Processor Interrupt, IPI) 可 用 于 实现 两 个 操作 系统 的 引导 。 一 个 CPU 的 本 地 APIC 可 产生 一 个 IPC， 被 另 一 个 CPU 当 作 一 个 正常 的 中 断 处 理 。CPU 也 会 产生 
一 个 特殊 的 INIT IP1， 如 果 一 个 CPU 收 到 一 个 INIT IP1， 它 会 从 INIT 1PI 传 递 的 地 址 开始 执行 引导 程序 。 这 种 性 质 可 以 被 CPU 热 插 拔 用 于 引导 另 一 个 操作 系统 内 核 。 此 外 ， 对 于 一 个 一 般 的 IP| 发 送 的 中 断 向 
量 ，CPU 处 理 时 会 将 它 当 作 正 常 中 断 ， 一 般 的 IPI 用 于 实现 CPU 和 OS 之 间 的 通信 。 


本 案例 开发 了 一 个 内 核 模块 ， 用 来 将 Nuttx 镜 像 加 载 到 内 存 中 ， 并 且 实 现 引 导 。 引 导 过 程 如 下 : 首先 ，Linux 正 常 引 导 ， 然 后 ，Nuttx 内 核 引 导 模 块 被 加 载 进 Linux 内 核 中 ， 并 且 打 开 Nuttx 监 视 器 。 
Nuttx 监 视 器 将 Nuttx 镜 像 加 载 到 内 存 中 ， 然 后 调用 cpu_down () 禁用 一 个 CPU 内 核 并 且 发 出 INIT IPI, INIT IPI 会 包含 Nuttx 镜 像 的 入 口 地 址 ， 从 而 将 Nuttx 引 导 到 被 禁用 的 那个 CPU 上 。 通 过 Nuttx 监 视 
器 ，Nuttx 可 以 动态 地 关闭 或 重新 打开 。 


(3) 设备 管理 


设备 分 为 实时 和 非 实时 两 类 。 非 实时 设备 由 普通 的 Linux 驱 动 实现 ， 而 实时 设备 由 Nuttx 驱 动 实现 ， 此 时 Linux 为 实时 设备 提供 的 驱动 将 会 被 禁用 。 实 时 设备 驱动 设置 |/O APIC 路 由 转发 中 断 给 Nuttx。 当 
引导 Nuttx 时 ，Linux 会 将 IRQ (FEER) 表 (记录 每 个 设备 的 中 断 请 求 号 ) 作为 参数 传递 给 Nuttx。 这 将 被 Nuttx 设 备 驱动 用 来 分 配 IRQ 号 ， 以 避免 |RQ 发 生 冲 突 。 如 果 两 个 不 同 的 设备 正好 共享 一 个 相同 
的 IRQ 号 ， 对 于 PCI (外 部 设备 互联 总 线 ) 设备 来 说 是 常见 的 ， 这 时 将 会 禁用 非 实 时 设备 。 但 是 ， 如 果 硬 件 支 持 MSI (Message Signaled Interrupt， 消 息 信 号 中 断 ) ， 那 么 可 以 编程 让 设备 产生 的 中 断 绕 过 
I/O APIC 直 接 交 付 给 相应 的 CPU 内 核 ， 这 样 就 可 以 有 效 地 解决 冲突 问题 。 


(4) 其 他 


本 案例 使 用 MP 体系 结构 的 共享 内 存 实现 了 双向 消息 的 FIFO。 通 信 中 最 大 的 问题 是 同步 Linux 和 Nuttx 之 间 的 两 个 任务 ， 本 案例 实现 了 一 个 自 旋 锁 模块 ， 分 别 用 于 同步 inux 和 Nuttx 上 的 两 个 任务 。 


7.1.3 结合 OROCOS 的 实时 性 实现 


“OROCOS" 是 由 “Open Robot Control Software” 计 划 里 各 词 的 部 分 字母 缩写 而 成 的 。 该 计划 的 目的 是 要 开发 一 种 通用 且 开 源 的 机 器 人 控制 模块 化 架构 。OROCOS 计 划 由 4 个 C++ 库 组 成 : 实时 工 
具 集 、 运 动 学 与 动力 学 算法 集 、 贝 叶 斯 过 滤 库 及 OROCOS 组 件 库 。 实 时 工具 集 (RIT) 不 仅 是 一 个 应 用 程序 ， 还 提供 了 基础 机 能 来 支持 使 用 C++ 以 构建 机 器 人 应 用 。OROCOs 的 优势 体现 在 实时 、 在 线 交 互 
和 基于 组 件 式 的 应 用 方面 。 


将 ROS 和 OROCOS 有 效 地 结合 在 一 起 ， 可 综合 各 自 的 优点 。 利 用 OROCOS 实 现 底 层 控制 ， 可 以 保证 系统 的 实时 性 ; 利用 ROS 实 现 上 层 控制 ， 可 以 充分 利用 ROSs 丰 语 的 功能 包 。 利 用 OROCOS 组 件 实 现 
的 控制 系统 被 压缩 在 一 个 ROS 节 点 中 ,没有 将 OROCOS 组 件 的 接口 暴露 给 ROS， 只 是 将 一 些 OROCOS 端 口 映射 到 ROS 主 题 上 。OROCOS RTT 端 口 与 ROS 消 息 定义 达成 一 致 ， 即 可 实现 数据 通信 。 


目前 已 有 一 个 开源 的 OROCOS RTT/ROS 集 成 包 (orocos/rtt_ros integration) ， 下 载 地 址 为 : https://github.com/orocos/rtt_ros integration。 其 包含 用 来 构建 OROCOS 与 ROS 消 息 系统 、ROS 参 
数 服务 器 通信 的 库 、 插 件 、 组 件 ， 具 体 功 能 如 下 。 


:ttt_tos: 用 于 引入 插件 、 包 装 脚本 和 launch 文 件 ， 从 而 使 得 可 以 通过 ROS 使 用 OROCOS。 它 包含 了 用 于 构建 和 运行 OROCOS 组 件 的 工具 ， 这 些 组 件 将 OROCOS 集 成 到 ROS 体 系 结 构 。 


- ttt_tosclock: 实时 安全 的 NTP (Network Time Protocol， 网 络 时 间 协 议 ， 用 于 同步 电脑 时 钟 到 一 些 时 间 参 考 上 ) 时 钟 测量 、ROS Time 结 构 构 建 、 基 于 模拟 时 钟 的 周期 性 RTT 活 动 。 它 包含 在 一 个 实时 操 
作 系 统 上 依据 不 同 的 时 间 源 测量 时 间 的 函数 。 当 在 一 个 ROS 网 络 中 运行 OROCOS 组 件 时 ， 保 证 不 同 机 器 之 间 的 时 钟 同步 很 重要 。 


- rtt_rosnode: 在 OROCOS 程 序 中 实例 化 一 个 ROS 节 点 的 播 件 。 
:ttt_tospatam: 用 于 同步 ROS 参 数 服务 器 和 OROCOS 组 件 属 性 的 插件 。 


` rtt_roscomm: ROS 消 息 typekit 产 生 器 、 用 来 发 布 和 订阅 ROS 主 题 ， 调 用 和 响应 ROS 服 务 的 OROCOS 插 件 。 


- rtt_tosdeployment: 一 个 RTT 服 务 ， 它 会 将 通用 的 DeploymentComponent 操 作 登 记 为 一 个 ROS 服 务 。 
- ttt_tospack: 用 来 定位 ROS 资 源 的 插件 。 

- ttt_tf: 一 个 RTT 插 件 ， 使 用 tf 使 得 RTT 组 件 可 以 转换 和 发 布 转换 结果 。 

` rtt_actionlib: RTT 使 能 的 action 服 务 ， 用 来 提供 来 自 ROS 和 RTT 融 合 系统 组 件 的 action。 

- rtt_dynamic_reconfigure: 一 个 实现 在 运行 时 更 新 属性 的 动态 重 配置 服务 插件 。 

“ttt_ros_msgs: 通过 这 些 插件 使 用 ROS 的 .msg 和 .stv 类 型 的 数据 。 


- ttt_ros_integration: 将 上 述 这 些 包 组 合 为 catkin 可 以 编译 的 单一 的 逻辑 包 。 


7.2 ”遗产 代码 的 ROS 集 成 


在 各 类 智能 机 器 人 开发 中 ， 开 放 的 ROS 确 实 具 有 不 少 优势 ， 但 需要 解决 ROS 如 何 有 效 集成 特定 机 器 人 已 有 代码 的 问题 。 遗 产 代 码 已 经 过 了 应 用 验证 ， 专 用 特性 可 保障 其 运行 的 高 效 性 ， 如 果 能 够 有 效 地 
将 现 有 机 器 人 系统 的 遗产 代码 与 ROS 相 结合 ， 那 么 既 可 以 针对 特定 机 器 人 使 用 原 有 系统 ， 又 能 够 有 效 利用 ROS 丰 语 的 功能 包 。 


将 遗产 代码 与 ROS 之 间 实 现 连 接 有 多 种 方法 。Fabio Dalla Libera 等 人 开发 的 ROSlink 是 一 个 开源 项 目 ， 用 于 解决 现 有 系统 使 用 ROS 作 为 接口 时 产生 的 冲突 问题 。ROS 官 网 上 给 出 的 rosbridge 也 可 用 于 
实现 遗产 代码 与 ROS 之 间 的 兼容 ， 为 非 ROS 程 序 提供 了 一 个 JSON APl， 用 于 实现 与 ROS 之 间 的 连接 。 


7.2.1 ROSlink 简 介 


ROSlink 是 一 个 实现 将 遗产 代码 整合 到 ROS 系 统 的 开源 项 目 ， 主 要 用 来 实现 现 有 软件 与 ROS 之 间 的 交互 ， 并 且 在 实际 中 不 需要 在 运行 遗产 代码 的 机 器 上 安装 ROS。 


ROSlink 人 允许 使 用 ROS 连 接 不 能 运行 ROS 的 遗产 系统 ， 使 用 ROSlink 的 遗产 系统 通过 发 布 /订阅 ROS 主 题 、 调 用 /提供 服务 形式 来 实现 集成 。ROSlink 接 口 与 roscpp 接 口 完全 对 应 ， 当 使 用 ROSlink 时 , 不 
需要 学 习 任 何 新 的 APl。 使 用 ROSlink， 可 以 继续 使 用 遗产 代码 中 的 数据 ， 如 可 以 发 布 一 个 std: : pair 或 一 个 自 定义 的 类 。ROSlink 提 供 了 遗产 数据 类 型 和 ROS 类 型 (由 .msg 和 .srv 文 件 产生 ) 之 间 的 封装 函 
数 。 


ROSlink 的 设计 准则 规定 了 一 组 独 有 的 特征 ， 使 得 不 能 安装 ROS 的 系统 也 可 以 和 ROS 互 联 。 首 先 ，ROSlink 要 求 对 遗产 代码 做 非常 有 限 的 改变 ， 所 以 要 求 工程 是 独立 的 ， 没 有 依赖 关系 ， 这 在 遗产 代码 中 
可 能 会 难以 满足 。 其 次 ， 已 经 在 遗产 代码 中 使 用 的 任何 数据 类 型 ， 都 可 以 在 主题 和 服务 中 使 用 。ROSlink 允 许 在 运行 时 对 遗产 代码 和 和 ROS 之 间 的 拓扑 进行 变更 ， 人 允许 遗产 代码 使 用 ROS 的 名 字 映 射 系统 ， 并 
且 对 遗产 代码 命令 行 参数 无 任何 强制 约束 。 最 后 ， 通 过 简化 使 用 一 系列 接近 ROS 编 程 接口 的 APl，ROSlink 简 化 了 连续 移植 代码 到 ROS 系 统 的 过 程 。 


7.2.2 ”ROSlink 实 现 概述 


ROSlink 主 要 用 来 实现 现 有 软件 与 ROS 之 间 的 交互 ， 并 且 在 实际 中 不 需要 在 运行 遗产 代码 的 机 器 上 安装 ROS。 在 软件 开发 的 过 程 中 ， 设 定 了 以 下 两 个 基本 目标 : 1) 最 少 化 用 户 需要 学 习 的 API。2) 最 小 
化 对 现 有 系统 的 更 改 。 


为 了 达到 这 一 目标 ，ROSlink 采 用 C++ 实现 ， 它 为 现 有 系统 提供 了 一 组 用 C+ + 实现 的 API， 其 本 质 与 roscpp 相 同 ， 这 样 会 使 熟悉 roscpp 的 用 户 很 容易 地 学 习 ROSlink。 为 了 实现 最 小 化 软件 安装 和 代码 
修改 ， 可 将 代码 实现 分 解 为 两 个 构件 ， 如 图 7-6 所 示 ， 一 个 是 轻 量 级 独立 的 C++ 库 helium， 其 供 遗 产 代码 使 用 ; 另 一 个 构件 是 用 分 布 式 ROS 包 实现 的 库 roslink， 用 于 实现 ROS 和 遗产 代码 之 间 数 据 的 封闭 传 
送 处 理 。 在 目前 的 实现 中 ， 这 两 个 构件 之 间 的 通信 和 是 通过 TCP/IP 来 实现 的 ， 以 后 如 果 要 采用 其 他 的 通信 方式 ， 如 12C， 则 需要 对 一 些 类 的 实现 进行 相应 的 更 改 。 
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图 7-6 ”ROSlink 具 体 实 现 


这 种 划分 为 两 个 构件 的 思想 与 rosbridge 类 似 。rosbridge 建 立 了 一 个 TCP/IP 服 务 器 ， 接 收 用 JSON 语 法 格式 编写 的 指令 ， 这 些 指令 会 发 布 /订阅 主题 、 调 用 ROS 服 务 。 因 此 客户 端 可 以 通过 TCP/IP 连 接 发 
送 JSON 对 象 实现 与 ROS 的 交互 ， 其 中 JSON 对 象 会 包含 rosbridge 协 议 中 指定 的 域 。ROSlink 与 rosbridge 设 计 目 标的 不 同 之 处 体现 在 : 1) rosbridge 人 允许 非 ROS 客 户 端 (如 用 JavaScript 编 写 的 网 页 ) 去 访问 
已 有 的 ROS 代 码 ; 而 ROSlink 主 要 实现 在 新 工程 中 重新 使 用 遗产 代码 。2) rosbridge 不 允许 客户 端 去 提供 ROS 服 务 ， 只 能 调用 ROS 已 有 的 服务 ;而 ROSlink 人 允许 ROS 节 点 调用 遗产 代码 提供 的 服务 。 
3) rosbridge 的 使 用 者 将 其 数据 类 型 转换 为 JSON 消 息 ，rosbridge 主 要 给 它 的 使 用 者 提供 一 些 简单 的 APl， 从 而 加 速 开发 进程 。 


7.2.3 ”ROSlink 设 计 原 理 


为 了 快速 、 容 易 地 将 现 有 系统 集成 到 ROS 中 ，ROSlink 设 置 了 以 下 设计 准则 。 
> 最 小 化 heli um 组件 的 依赖 集 。 
- helium API 和 toscpp API 之 间 兼 容 。 
. 动态 创建 ROS 发 布 者 、 订 阅 者 、 服 务 客户 端 与 服务 器 的 能 力 。 
“ 运行 时 改变 网 络 拓扑 结构 的 健 半 性 。 
完全 支持 ROS 名 字 映 射 系统 。 
. ROS 和 遗产 代码 之 间 的 封装 传送 处 理 完全 独立 于 遗产 代码 。 
下 面 将 对 这 些 准则 的 实现 进行 简要 分 析 。 
(1) 依赖 性 


除了 网 络 和 多 线程 ， 与 遗产 代码 连接 的 helium 库 是 独立 的 。 编 译 helium 库 时 ， 在 POSIX 系 统 上 使 用 Berkeley sockets 网 络 实现 ， 在 Windows 上 使 用 Winsock API 网 络 实现 。 同 样 ， 对 于 多 线程 ， 在 
POSIX 系 统 上 使 用 Pthreads， 在 Windows 上 使 用 本 地 Windows 线 程 。 对 于 具有 独立 代码 特性 的 类 ， 可 以 在 Pthreads 或 Windows APl 中 重新 实现 。 尽 量 不 要 使 用 其 他 会 增加 代码 移植 量 的 库 ， 尤 其 是 Boost 
库 ， 以 避免 当前 Boost API 与 代码 中 老 版 本 API 可 能 存在 的 不 兼容 问题 。 来 自 cmake 文 件 的 库 ， 当 cmake 文 件 无 效 时 ， 可 在 Visual Studio 工 程 中 手动 编写 ，Linux 或 QNX 会 为 系统 提供 Makefiles 文 件 。 用 户 
也 可 以 选择 不 将 这 些 代码 编译 成 库 ， 而 是 将 helium 代 码 与 遗产 代码 一 起 编译 。 


(2) API 


暴露 给 遗产 系统 的 AP 上 与 映射 的 roscpp API 应 尽 可 能 地 接近 。 每 一 个 ROS 对 象 ， 如 ros: : NodeHandle 或 ros: : Publisher， 在 helium 中 都 会 有 对 应 的 副本 ， 如 legacy: : NodeHandle 或 legacy: : 
Publisher， 成 员 函 数 也 能 与 ROS 等 价 映射 。 此 外 helium 还 提供 了 ROS 的 特性 ， 如 在 SubscriberStatusCallback 中 会 发 布 一 个 单独 的 订阅 者 。 当 ROS 出 现 一 个 异常 时 ， 遗 产 代码 同样 也 会 出 现 一 个 异常 。 在 
helium 中 设置 、 读 取 和 搜索 参数 也 可 采用 与 ROS 同 样 的 方式 。 这 将 会 带 来 很 大 的 优势 : 这 会 使 得 遗产 系统 支持 ROS， 通 过 本 地 支持 就 可 实现 用 ROS 命 名 空间 代 蔡 遗产 系统 命名 空间 。 


为 了 实现 与 ROSlink 通 信 ，helium API 引 入 了 一 个 新 对 象 legacy: : Link, legacy: : Link 对 象 通过 构造 器 会 指定 一 个 唯一 的 名 字 ， 当 ROS 节 点 实例 化 一 个 ros: : Link 对 象 ( 由 ROSlink 包 提供 ) AY, 
它们 之 间 就 会 自动 建立 通信 。 从 概念 上 讲 ， 在 遗产 代码 中 构建 legacy: : NodeHandle 的 操作 和 在 声明 ros: : Link 对 象 节点 中 构建 ros: : NodeHandle 的 操作 是 一 样 的 。 遗 产 代码 使 用 legacy: : 
NodeHandle 的 成 员 advertiseservice 提 供 的 服务 被 看 作 是 ros: : Link 对 象 所 在 ROS 节 点 提供 的 服务 。 同 时 ， 一 个 单独 的 遗产 程序 可 声明 多 个 legacy: : Link 对 象 。 因 此 ， 每 个 legacy: : NodeHandle 使 
用 的 link 都 可 以 通过 传递 一 个 legacy: : Link 对 象 引 用 给 legacy: : NodeHandle 构 造 器 来 指定 。 


(3) 动态 创建 和 销毁 


在 ROs 中 ， 当 一 个 ros: : service 对 象 被 初始 化 时 ， 系 统 就 会 广播 该 服务 ， 当 最 后 一 个 ros: : Service 副 本 被 销毁 时 ， 将 自动 不 再 广播 该 服务 。 同 理 ， 对 于 helium 维 护 的 发 布 者 、 订 阅 者 、 客 户 端 ， 其 
创建 和 销毁 也 均 有 效 。 


即使 ROS 节 点 包含 提供 遗产 服务 的 ros: : Link， 且 节点 是 运行 的 ， 也 只 有 对 应 的 legacy: : Service 被 注册 时 ， 对 应 的 ROS 服 务 才 会 有 效 。 同 样 ， 当 legacy: : Service 的 最 后 一 个 副本 被 销毁 时 ， 或 者 
legacy: : Link 和 ros: : Link 之 间 的 连接 断 开 时 ， 服 务 将 不 会 再 被 广播 。 


(4) 网 络 拓扑 连接 


遗产 系统 和 ROS 之 间 的 数据 交换 是 通过 helium 库 和 ROS 节 点 之 间 的 通信 来 实现 的 。 具 体 的 通信 过 程 可 通过 遗产 代码 声明 的 legacy: : Link 对 象 和 ROS 对象 创建 的 同名 ros: : Link 对 象 来 实现 。ROSlink 
没有 实例 化 顺序 ， 所 以 ROS 节 点 和 遗产 代码 运行 的 先后 顺序 不 确定 ， 并 且 两 个 构件 并 不 需要 知道 一 对 实体 的 位 置 (主机 和 和 TCP 端口 ) 。 事 实 上 ，helium 提 供 了 Imaster 程 序 ， 其 扮演 了 一 个 DNS 服务 器 的 角 
色 ， 与 roscore 的 工作 方式 相同 ， 人 允许 节点 之 间 通 信 ， 并 且 不 需要 预先 知道 其 他 的 主机 名 和 端口 。 具 体 而 言 ， 当 一 个 legacy: : Link 或 ros: : Link 对 象 被 创建 时 ， 它 会 自动 地 在 Imaster 服 务 器 上 注册 ， 并 且 
当成 对 的 实体 有 效 时 ， 对 象 会 被 通知 需要 连接 的 主机 名 和 端口 号 ， 从 而 两 个 对 象 之 间 会 建立 直接 连接 。 该 方法 具有 灵活 地 动态 重 配置 网 络 拓 扑 的 优点 。 图 7-7 提 供 了 4 种 可 能 的 场景 示例 ， 具 体 如 下 。 


a) 每 一 个 遗产 程序 使 用 legacy: : Link 连 接 对 应 ROS 节 点 声明 的 ros: : Link (一 对 一 ) 。 

b) 所 有 遗产 程序 使 用 一 个 ROS 节 点 提供 的 同一 个 ROSIink (多 对 一 ) 。 

c) 一 个 遗产 程序 通过 多 个 legacy: : Link 使 用 多 个 ROS 节 点 的 ros: : Link， 提 供 概念 上 的 不 同 服务 (一 对 多 ) 。 

d) 一 种 混合 方法 ， 多 个 单一 遗产 服务 映射 到 相同 的 ROS 节 点 ， 同 时 对 于 一 个 高 带宽 数据 流 的 遗产 程序 ， 可 连接 到 运行 在 不 同 机 器 的 ROS 节 点 上 (b、c 的 组 合 ) 。 


由 于 Imaster 提 供 了 上 述 动态 绑 定 方法 ， 可 在 运行 时 更 改 配置 ， 如 在 多 个 机 器 之 间 平 衡 负载 ， 或 者 弥补 一 个 机 器 的 暂时 性 失效 。 
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图 7-7 4 个 拓扑 实例 的 UML 调 度 图 解 
(5) 名 字 映 射 


ROS 为 主题 和 服务 提供 了 一 个 名 字 映 射 机 制 ， 其 允许 硬 加 一 个 完整 的 命名 空间 ， 因 此 可 用 于 整合 多 个 异 构 系统 并 且 不 会 出 现 命 名 冲突 。 此 外 ， 节 点 、 主 题 、 参 数 的 名 字 可 以 通过 命令 行 或 launch 文 件 重 
映射 ， 从 而 可 以 在 不 同 的 配置 下 执行 相同 的 代码 。 在 创建 NodeHandle 时 可 以 指定 在 特定 NodeHandle 子 树 下 声明 额外 的 名 字 映 射 ， 其 还 可 为 每 一 个 节点 创建 一 个 特别 的 私有 命名 空间 。ROS 可 通过 命令 行 
为 命名 空间 中 的 参数 赋值 ，ROSlink 保 持 了 该 特性 ， 在 创建 legacy: : NodeHandle 对 象 时 ， 也 可 以 指定 重 映 射 。ROSlink 模 型 中 ， 在 遗产 代码 中 创建 的 节点 在 概念 上 依赖 包含 对 应 ros: : Link 的 ROS 节 
点 ， 因 此 ， 当 一 个 包含 ros: : Link 的 节点 使 用 重 映射 的 参数 运行 时 ， 该 参数 也 会 通过 对 应 的 legacy: : Link 在 该 遗产 代码 中 使 用 。 这 就 允许 ROSlink 用 户 可 以 很 容易 地 根据 ROS 实 现 遗 产 系统 的 重 映 射 ， 而 
不 需要 重新 运行 遗产 代码 。 此 外 ， 它 使 得 映射 操作 无 需 给 遗产 代码 传递 命令 行 参数 ， 因 为 它 可 能 采用 与 ROs 重 映射 和 参数 赋值 不 兼容 的 方式 解析 命令 行 参 数 。 


(6) 封装 与 传送 处 理 


在 ROS 中 ， 主 题 和 服务 消息 类 型 通常 采用 一 种 直观 的 定义 语言 。 脚 本 可 用 来 为 每 一 个 特定 的 编程 语言 产生 消息 文件 ， 其 在 C++ 中 是 头 文件 形式 。 生 成 的 代码 可 通过 提供 序列 和 逆序 列 方法 ， 实 现 网 络 中 
消息 数据 的 传输 。 当 连接 遗产 代码 和 和 ROS 时， 可 以 在 装 有 ROS 的 系统 上 产生 头 文件 ， 然 后 将 它们 包含 到 遗产 代码 中 ， 使 用 ROS 产 生 的 数据 类 型 代替 原始 的 数据 类 型 或 者 当 使 用 ROS 方 法 时 插入 原始 遗产 数 
据 类 型 和 和 ROS 数据 类 型 之 间 的 映射 函数 。 还 可 以 手动 配置 ROS 需 要 的 遗产 数据 类 型 的 序列 和 逆序 列 方法 。 


实现 准则 之 一 是 尽 可 能 保证 遗产 代码 不 被 改变 ，ROSlink 为 此 提供 了 一 种 支持 方法 。helium 对 象 使 用 的 主题 和 服务 消息 可 使 用 任何 数据 类 型 ， 因 此 可 以 直接 使 用 遗产 数据 类 型 。helium: : Link 和 对 应 
的 ros: : Link 之 间 所 有 数据 的 序列 和 逆序 列 化 均 默认 采用 数据 类 型 的 “< <” 和 “> >” 操 作 ， 需 要 其 他 序列 化 时 ， 可 采用 特定 数据 类 型 的 helium: : write 和 helium: : read 函 数 实现 。 另 外 ， 遗 产 数 据 类 
型 和 ROS 类 型 之 间 的 转换 可 在 ROS 节 点 定义 ros: : Link 时 执行 。 


7.3 ”机 器 人 任务 级 编程 


对 于 机 器 人 任务 级 编程 ， 操 作者 只 需要 给 出 机 器 人 的 初始 状态 和 目标 状态 即 可 ， 机 器 人 自身 来 实现 任务 规划 。 任 务 级 编程 可 以 实现 一 定 层次 的 共享 编程 ， 机 器 人 提供 一 定 的 专业 知识 ， 或 者 处 理 一 些 细 
节 问 题 ， 减 轻 操 作者 的 负担 ， 此 外 操作 者 还 可 以 参与 机 器 人 的 感知 。 


目前 ， 针 对 机 器 人 编程 仍 需要 很 强 的 专业 知识 ， 如 果 ROS 可 以 实现 任务 级 编程 ， 那 么 非 专 业 人 员 也 可 以 实现 对 机 器 人 的 编程 。 ROS 中 的 rviz 工 具 ， 它 提供 了 可 视 化 和 机 器 人 控制 的 可 扩展 接口 。 如 果 能 在 
rviz 环 境 中 实现 任务 级 编程 ， 那 么 操作 者 只 需要 调整 图 形 化 界面 ， 就 可 以 非常 简单 且 快 速 地 调整 、 执 行 机 器 人 应 用 。 


2015 年 ， 由 Stephen Hart 等 提出 的 功能 可 见 性 模板 (Affordance Template, AT) ROS 包 ,实现 了 任务 级 编程 。 它 扩展 了 rviz 交 互 式 标 记 ， 人 允许 操作 者 在 以 对 象 为 中 心 的 坐标 系 中 指定 多 个 末端 执行 器 


航 点 位 置 和 抓 取 位 置 ， 并 且 可 以 调整 这 些 航 点 位 置 去 满足 任务 运行 时 要 求 。 此 外 ，AT 包 使 用 JSON 描 述 格式 存储 任务 说 明 。 因 此 功能 可 见 性 模板 包 提供 了 一 个 机 器 人 通用 的 ROS 工 具 ， 其 适用 于 构建 半 自 
治 、 基 于 操作 的 应 用 。 


Lal 合作 


功能 可 见 性 最 初 由 Gibson 在 心理 学 文献 中 提出 ， 作 为 一 种 描述 认 知 结构 的 手段 ， 用 于 为 环境 分 配 功能 。 功 能 可 见 性 描述 环境 或 对 象 ， 通 过 一 个 特定 的 agent 提 供 了 一 个 action。 如 果 感 知 环境 中 的 一 块 
区 域 提供 了 一 个 “ 坐 ” 的 功能 ， 则 表明 该 区 域 包含 一 个 平面 (agent) ， 它 可 以 是 椅子 、 岩 石 ， 或 者 地 面 。 因 此 ， 在 概念 上 这 对 可 编程 机 器 人 同样 适用 ， 操 作者 (或 机 器 人 自身 ) 可 类 似 地 分 配 一 些 action 
给 传感器 数据 中 可 识别 的 对 象 ， 机 器 人 通过 执行 一 些 action 完 成 可 辨识 的 目标 。 


功能 可 见 性 模板 是 一 个 计算 架构 ， 人 存在 于 图 形 化 3D 沉 浸 式 环境 中 ， 在 以 对 象 为 中 心 的 坐标 系 中 ， 可 以 提供 人 类 可 调整 的 机 器 人 任务 目标 和 参数 。 一 个 典型 的 场景 是 ， 计 算 机 中 包含 机 器 人 模型 和 它 收 集 
的 传感器 数据 ， 操 作者 可 根据 任务 需求 移动 模板 到 合适 的 位 置 ， 并 且 可 根据 在 当前 运行 环境 中 感知 的 需求 调整 模板 目标 和 参数 。 模 板 参数 捕获 任务 中 关键 的 角度 ， 并 且 会 充分 利用 工具 实现 机 器 人 和 操作 者 
之 间 的 自治 权 共 享 ， 其 适用 于 不 可 靠 网 络 或 通信 退化 场景 中 的 控制 。 


AT 包 旨 在 为 包含 一 个 或 多 个 末端 执行 器 的 机 器 人 标准 化 应 用 编程 。 与 机 器 人 导航 、 运 动 规划 、 机 器 视觉 (OpenCV) 、3D 感 知 (PCL) 等 标准 的 ROS 功 能 包 不 同 ， 功 能 可 见 性 模板 存在 于 这 些 工具 之 
上 ， 可 根据 需要 整合 这 些 功 能 。 


功能 可 见 性 模板 是 在 DARPA 机 器 人 挑战 赛 中 开发 的 ， 此 后 其 成 功 地 部 署 在 多 个 平台 上 ， 其 中 包括 NASA Valkyrie、Robonaut2 类 人 型 机 器 人 、 德 克 萨 斯 大 学 的 梦想 家 机 器 人 、Willow Garage 的 PR2 


7.3.2 ”功能 可 见 性 模板 示例 


图 7-8 为 功能 可 见 性 模板 的 一 个 示例 ， 将 轮子 旋转 AT 模 板 注 册 到 NASA-JSC Valkyrie 机 器 人 的 传感器 数据 上 ， 从 而 实现 机 器 人 关闭 阀门 的 功能 。 其 中 白色 圆 环 为 传感器 采集 到 的 阀门 数据 ， 以 该 圆 环 中 
心 为 坐标 系 中 心 点 ， 图 7-8 中 的 手 状 图 标 展示 了 机 器 人 末端 执行 器 的 航 点 位 置 ( 预 抓 取 、 抓 取 、 和 转换 目标 等 ) 和 一 个 抓 取 位 置 (打开 、 关 闭 ) 。 航 点 位 置 是 顺序 的 ， 操 作者 可 以 沿 着 路 径 正 向 或 逆向 移动 ， 也 
可 以 运行 整个 路 径 。 在 Rviz 中 ， 该 模板 可 通过 图 中 显示 的 六 个 自由 度 控制 整个 路 径 移动 ， 也 可 通过 手 状 图 标 调整 单独 的 路 径 点 。 


图 7-9a 中 ，NASA-JSC Valkyrie 机 器 人 位 于 阀门 前 方 。 图 7-9b 中 ，Rviz 窗 口 展示 了 机 器 人 模型 和 从 机 器 人 头 戴 式 传感器 Ensenso 中 采集 的 点 云 数 据 。 图 7-9c 中 ， 在 Rviz 内 操作 者 可 以 清楚 地 识别 机 器 人 
工作 空间 中 的 阀门 ， 手 动 注册 一 个 轮子 旋转 AT 模 板 到 阀门 位 置 ， 并 可 在 定制 的 Rviz 控 制 板 中 通过 滚动 条 调整 它 的 大 小 。 此 外 ， 轮 子 旋转 AT 模 板 允 许 操作 者 使 用 交互 式 图 标 调整 手 预 抓 取 、 抓 取 和 关闭 目标 阀 
门 的 航 点 位 置 。 如 图 7-9d 所 示 ， 当 航 点 位 置 设置 好 后 ， 对 应 的 机 器 人 手臂 路 径 可 在 Rviz 中 显示 ， 如 果 可 以 得 到 有 效 路 径 ， 则 会 通过 机 器 人 运动 反馈 给 操作 者 。 通 过 Movelt 得 到 路 径 ， 如 果 操 作者 对 Rviz 控 制 
板 中 展示 的 路 径 满意 ， 则 可 将 其 作为 执行 路 径 。 





图 7-8 为 Valkytie 机 器 人 实例 化 的 一 个 轮子 旋转 功能 可 见 性 模板 





图 7-9 ”在 Rviz 内 为 Valkytie 机 器 人 添加 一 个 轮子 旋转 AT 模板 


7-10 展 示 了 Valkyrie 使 用 模板 关闭 阀门 的 过 程 。 
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图 7-10 Valkytie 利 用 轮子 旋转 AT 模板 关闭 阀门 


= ti 
ml 本 十 /一 

Eas | f | whe ata 

Aru! i 


AT 包 的 实现 架构 如 图 7-11 所 示 。 功 能 可 见 性 模板 服务 器 节点 从 存储 在 磁盘 上 的 相应 库 中 加 载 有 效 模板 和 机 器 人 配置 ， 并 使 用 SON 数 据 结构 通过 ZeroMQ (ZMQ) 将 信息 发 送 至 Rviz 控 制 板 。 在 Rviz 控 
制 面板 中 ， 操 作者 可 以 针对 特定 机 器 人 选择 模板 ， 这 些 信 息 会 反馈 到 服务 器 中 ， 服 务 器 将 对 应 模板 实例 化 为 一 个 Python 类 ， 通 过 ROS 节 点 作为 独立 进程 运行 。 根 据 配 置 文件 中 定义 的 映射 关系 ， 实 例 化 类 为 
显示 对 象 和 机 器 人 末端 执行 器 显示 Rviz 交 互 式 标记 ， 还 会 包含 模板 定义 的 配置 。 该 进程 允许 实例 化 如 图 7-8 中 展示 的 轮子 旋转 AT， 并 在 中 心 位 置 显示 白 轮 ， 显 示 机 器 人 末端 执行 器 的 航 点 位 置 ， 以 及 交互 式 


标记 的 配置 。 


一 旦 在 Rviz 中 实例 化 模板 ， 操 作者 便 可 与 其 进行 交互 ， 移 动 它 到 指定 位 置 ， 根 据 需要 缩放 模板 实例 化 对 象 ， 调 整 未 端 执 行 器 的 位 置 。 当 操作 者 对 配置 满意 时 ， 利 用 Movelt 计 算出 一 个 路 径 
JointTrajectory 消 息 ， 并 发 送 给 机 器 人 ， 这 样机 器 人 就 可 以 按 此 路 径 执行 。 操 作者 对 模板 或 机 器 人 配置 参数 进行 修改 后 ， 更 新 信息 会 反馈 到 服务 器 ， 并 且 会 存储 在 库 中 以 便 将 来 使 用 。 
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图 7-11 AT 实现 架构 


7.3.4 ”Rviz 用 户 界面 


为 了 在 机 器 人 上 执行 任务 级 行为 ，Rviz 提 供 了 两 个 用 于 与 AT 交互 (创建 或 修改 AT) 的 方法 : 定制 的 Rviz 控 制 面板 和 上 下 文 按钮 (模板 实例 化 后 有 效 ) 。 
(1) Rviz 控 制 面板 


图 7-12 为 Rviz 功 能 可 见 性 控制 面板 。 在 主 窗口 中 ，UT Dreamer 机 器 人 使 用 拾取 与 放置 模板 。 右 侧 窗 口 展示 了 AT 服务 器 连通 性 的 基本 信息 和 已 经 初始 化 的 模板 。 任 何 模板 在 任意 时 刻 都 可 初始 化 多 个 实 
例 。 操 作者 也 可 删除 存在 的 模板 。 右 侧 窗 口 的 上 半 部 分 允许 操作 者 为 机 器 人 分 配 AT; 第 二 个 tab 保 人 存 机 器 人 的 配置 文件 yam| 参 数 。 右 侧 窗 口 的 下 半 部 分 展示 了 AT 库 ， 实 例 化 一 个 新 模板 ， 操 作者 只 需要 在 模 
板 列表 中 选择 一 个 图 标 ， 对 应 的 模板 就 会 出 现在 主 窗口 中 ; 第 二 个 tab 可 以 用 来 调整 对 象 的 大 小 ， 或 者 管理 模板 末端 执行 器 航 点 的 位 置 。 操 作者 可 以 指定 机 器 人 沿 着 路 径 在 任意 时 刻 以 任意 方向 移动 任意 
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b) 右 侧 面板 上 半 部 分 c) 二 侧面 板 下 半 部 分 


图 7-12 UT Dreamet 机 器 人 使 用 拾取 与 放置 模板 (ATH DREAM) 


(2) 上 下 文 按钮 

在 Rviz 主 窗口 显示 的 对 象 或 航 点 位 置 上 右 击 ， 可 以 为 操作 者 提供 一 些 有 效 的 特征 ， 具 体 如 下 。 
` 隐藏 /显示 控制 : 为 了 降低 Rviz 环 境 的 杂乱 性 ， 操 作者 可 以 选择 不 显示 控制 。 一 般 将 模板 放置 到 适当 位 置 ， 或 者 微调 末端 执行 器 位 置 时 才 会 选择 显示 控制 。 
> 增加 /删除 /移动 航 点 位 置 : 操作 者 可 以 在 末端 执行 器 路 径 上 增加 、 删 除 航 点 位 置 ， 或 者 交换 序列 中 的 任意 一 对 航 点 。 


* 更 改 抓 取 位 置 : 操作 者 可 以 选择 存储 的 任 一 航 点 位 置 作为 抓 取 位 置 ，Movelt 产 生 的 SRDF 文 件 中 的 所 有 位 置 都 是 有 效 选项 。 


* 保存 修改 : 将 操作 者 对 模板 的 任何 修改 保存 到 AT 库 中 。 


“ 政变 路 径 : 操作 者 在 任意 时 刻 可 选择 与 该 模板 相关 路 径 中 的 任何 一 个 作为 执行 路 径 。 


74 ”本章 小 结 


本 章 从 多 个 方面 介绍 了 基于 ROS 的 智能 机 器 人 系统 开发 方法 。 针 对 实时 性 ， 可 以 通过 混合 实时 ROS 体 系 结构 RGMP-ROS 或 结合 OROCOS 实 现 。 目 前 机 器 人 领域 包含 大 量 专用 高 效 的 遗产 代码 ， 通 过 
ROSlink、rosbridge 等 可 以 有 效 地 实现 它们 之 间 的 连接 ， 这 样 就 可 以 使 原 有 系统 充分 利用 ROSs 丰 富 的 功能 包 。 通 过 功能 可 见 性 模板 可 以 实现 机 器 人 任务 级 编程 ， 这 样 非 专业 人 员 也 可 以 实现 对 机 器 人 编程 。 
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8.1 服务 智能 机 器 人 的 基本 情 ) 


8.1.1 服务 智能 机 器 人 的 现状 及 分 类 


1. 服 务 智 能 机 器 人 


根据 国家 机 器 人 联合 会 的 定义 ， 服 务 机 器 人 是 一 种 半 自 主 或 全 自主 工作 的 机 器 人 ， 它 是 能 够 帮助 人 类 完成 除 生产 制造 加 工 过 程 以 外 的 设备 。 服 务 智 能 机 器 人 作为 机 器 人 产业 的 新 兴 领 域 ， 高 度 融 合 智 
能 、 传 感 、 网 络 、 云 计算 等 创新 技术 ， 与 移动 互联 网 的 新 业态 、 新 模式 相 结 合 ， 从 而 促进 人 类 生活 的 智能 化 。 


目前 研制 中 的 服务 智能 机 器 人 的 智能 水 平 并 不 高 ， 只 能 说 是 尚且 处 于 服务 智能 机 器 人 的 初级 阶段 。 当 前 服务 智能 机 器 人 研究 中 的 核心 问题 有 两 个 方面 : 一 方面 是 ， 提 高 服务 智能 机 器 人 的 自主 性 ， 希 望 
服务 智能 机 器 人 进一步 独立 于 人 ， 具 有 更 为 友善 的 人 机 界面 ， 未 来 希望 操作 人 员 只 需 给 出 要 完成 的 任务 ， 机 器 人 就 能 自主 完成 它 ; 另 一 方面 是 ， 提 高 智能 机 器 人 的 适应 性 ， 提 高 智能 机 器 人 适应 环境 变化 的 
能 力 ， 加 强 机 器 人 与 环境 之 间 的 交互 关系 。 


在 各 国 的 智能 机 器 人 发 展 中 ， 美 国 的 技术 在 国际 上 一 直 处 于 领先 地 位 ， 其 技术 人 全面、 先进， 适应 性 也 很 强 ， 性 能 可 靠 、 功 能 全 面 、 精 确 度 高 ， 其 视觉 、 触 党 等 人 工 智能 技术 早已 广泛 应 用 到 了 各 个 领 
域 。 日 本 由 于 一 系列 的 扶植 政策 ， 各 类 机 器 人 包括 智能 机 器 人 的 发 展 非常 迅速 。 欧 洲 各 国 在 智能 机 器 人 的 研究 和 应 用 方面 在 世界 上 也 处 于 公认 的 领先 地 位 。 我 国 起 步 较 晚 ， 而 后 进入 了 大 力 发 展 的 时 期 ， 以 
期 以 机 器 人 为 媒介 物 推动 整个 制造 业 的 改变 ， 从 而 推动 整个 高 技术 产业 的 壮大 。 


在 服务 机 器 人 领域 的 研发 方面 ， 我 国 与 日 本 、 美 国 等 国家 相 比 起 步 较 晚 ， 但 在 国家 863 计 划 的 支持 下 ， 我 国 在 服务 机 器 人 领域 已 开展 了 大 量 技 术 研 究 和 产品 开发 ， 并 取得 了 不 少 成 果 。2015 年 4 月 ， 我 国 
科技 部 正式 印发 了 《服务 机 器 人 科技 友 展 “十 三 五 ”专项 规划 》， 提 出 “十 三 五 ”服务 机 器 人 重点 专项 安排 公共 安全 机 器 人 、 仿 生机 器 人 平台 、 医 疗 康复 机 器 人 和 模块 化 核心 部 件 等 4 个 方面 的 任务 。 


2. 服 务 智能 机 器 人 的 分 类 
服务 型 机 器 人 按照 用 途 可 分 为 娱乐 服务 机 器 人 、 家 庭 服务 机 器 人 和 专业 服务 机 器 人 。 
(1) 娱乐 服务 机 器 人 


娱乐 服务 机 器 人 以 供 人 观赏 、 娱 乐 为 目的 ， 具 有 机 器 人 的 外 部 特征 ， 可 以 像 人 或 某 种 动物 一 样 ， 同 时 具有 机 器 人 的 功能 ， 可 以 行走 或 完成 动作 ， 有 一 定 的 感知 能 力 、 语 言 能 力 和 交流 能 力 ， 如 机 器 人 歌 
手 、 足 球 机 器 人 、 玩 具 机 器 人 、 舞 蹈 机 器 人 等 。 


(2) 家 庭 服务 机 器 人 


家 庭 服务 机 器 人 用 于 在 家 庭 生活 中 承担 某 些 家 庭 事务 ， 如 照顾 孩子 学 习 玩 乐 ， 协 助 老人 更 衣 洗澡 ， 提 醒 病 人 按时 吃 药 ， 能 承担 诸如 端 条 倒 水 、 取 报纸 、 收 拾 餐具 、 倒 垃圾 、 吸 侍 、 探 窗户 等 家 务 活 ; 还 
可 以 进行 家 电 控制 、 防 盗 监测 、 安 全 检查 、 接 听 电 话 等 ， 甚 至 可 以 操作 电脑 以 从 互联 网 上 收集 新 闻 及 其 他 信息 服务 等 。 依 据 具 体 的 服务 能 力 可 将 其 分 为 保姆 机 器 人 、 豪 饪 机 器 人 、 清 洁 机 器 人 、 保 安 机 器 
人 、 秘 书 机 器 人 、 护 理 机 器 人 等 。 此 类 服务 智能 机 器 人 具有 自主 路 径 规 划 、 自 动 障碍 回避 、 自 行 执行 操作 等 能 力 。 


(3) 专业 服务 机 器 人 


专业 服务 机 器 人 主要 用 在 特定 的 场所 ， 以 代替 人 完成 特殊 的 任务 。 它 主要 包括 导游 机 器 人 、 医 用 机 器 人 、 模 特 机 器 人 、 管 道 机 器 人 、 教 育 机 器 人 、 生 命 科学 机 器 人 、 建 筑 机 器 人 、 邮 拾 和 送信 机 器 人 及 
加 油 机 器 人 等 。 其 中 医用 机 器 人 又 可 以 分 为 手术 机 器 人 、 诊 断 机 器 人 、 护 理 机 器 人 、 康 复 机 器 人 和 助残 机 器 人 等 。 此 类 服务 智能 机 器 人 具有 适应 特殊 环境 、 自 主 完成 特定 操作 等 的 能 力 。 
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8.1.1 服务 智能 机 器 人 的 现状 及 分 类 
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科技 部 正式 印发 了 《服务 机 器 人 科技 友 展 “十 三 五 ”专项 规划 》， 提 出 “十 三 五 ”服务 机 器 人 重点 专项 安排 公共 安全 机 器 人 、 仿 生机 器 人 平台 、 医 疗 康复 机 器 人 和 模块 化 核心 部 件 等 4 个 方面 的 任务 。 


2. 服 务 智能 机 器 人 的 分 类 
服务 型 机 器 人 按照 用 途 可 分 为 娱乐 服务 机 器 人 、 家 庭 服务 机 器 人 和 专业 服务 机 器 人 。 
(1) 娱乐 服务 机 器 人 


娱乐 服务 机 器 人 以 供 人 观赏 、 娱 乐 为 目的 ， 具 有 机 器 人 的 外 部 特征 ， 可 以 像 人 或 某 种 动物 一 样 ， 同 时 具有 机 器 人 的 功能 ,可 以 行走 或 完成 动作 ， 有 一 定 的 感知 能 力 、 语 言 能 力 和 交流 能 力 ， 如 机 器 人 歌 
手 、 足 球 机 器 人 、 玩 具 机 器 人 、 舞 蹈 机 器 人 等 。 


(2) 家 庭 服务 机 器 人 


家 庭 服务 机 器 人 用 于 在 家 庭 生活 中 承担 某 些 家 庭 事务 ， 如 照顾 孩子 学 习 玩 乐 ， 协 助 老人 更 衣 洗澡 ， 提 醒 病 人 按时 吃 药 ， 能 承担 诸如 端 条 倒 水 、 取 报纸 、 收 拾 餐具 、 倒 垃圾 、 吸 侍 、 探 窗户 等 家 务 活 ; 还 
可 以 进行 家 电 控制 、 防 盗 监测 、 安 全 检查 、 接 听 电 话 等 ， 甚 至 可 以 操作 电脑 以 从 互联 网 上 收集 新 闻 及 其 他 信息 服务 等 。 依 据 具 体 的 服务 能 力 可 将 其 分 为 保姆 机 器 人 、 豪 饪 机 器 人 、 清 洁 机 器 人 、 保 安 机 器 
人 、 秘 书 机 器 人 、 护 理 机 器 人 等 。 此 类 服务 智能 机 器 人 具有 自主 路 径 规 划 、 自 动 障碍 回避 、 自 行 执行 操作 等 能 力 。 


(3) 专业 服务 机 器 人 


专业 服务 机 器 人 主要 用 在 特定 的 场所 ， 以 代替 人 完成 特殊 的 任务 。 它 主要 包括 导游 机 器 人 、 医 用 机 器 人 、 模 特 机 器 人 、 管 道 机 器 人 、 教 育 机 器 人 、 生 命 科学 机 器 人 、 建 筑 机 器 人 、 邮 拾 和 送信 机 器 人 及 
加 油 机 器 人 等 。 其 中 医用 机 器 人 又 可 以 分 为 手术 机 器 人 、 诊 断 机 器 人 、 护 理 机 器 人 、 康 复 机 器 人 和 助残 机 器 人 等 。 此 类 服务 智能 机 器 人 具有 适应 特殊 环境 、 自 主 完成 特定 操作 等 的 能 力 。 


第 8 章 ”基于 ROS 的 服务 智能 机 器 人 设计 


8.1 服务 智能 机 器 人 的 基本 情 ) 


8.1.1 服务 智能 机 器 人 的 现状 及 分 类 


1. 服 务 智能 机 器 人 


根据 国家 机 器 人 联合 会 的 定义 ， 服 务 机 器 人 是 一 种 半 自 主 或 全 自主 工作 的 机 器 人 ， 它 是 能 够 帮助 人 类 完成 除 生产 制造 加 工 过 程 以 外 的 设备 。 服 务 智 能 机 器 人 作为 机 器 人 产业 的 新 兴 领 域 ， 高 度 融 合 智 
能 、 传 感 、 网 络 、 云 计算 等 创新 技术 ， 与 移动 互联 网 的 新 业态 、 新 模式 相 结 合 ， 从 而 促进 人 类 生活 的 智能 化 。 


目前 研制 中 的 服务 智能 机 器 人 的 智能 水 平 并 不 高 ， 只 能 说 是 尚且 处 于 服务 智能 机 器 人 的 初级 阶段 。 当 前 服务 智能 机 器 人 研究 中 的 核心 问题 有 两 个 方面 : 一 方面 是 ， 提 高 服务 智能 机 器 人 的 自主 性 ， 希 望 
服务 智能 机 器 人 进一步 独立 于 人 ， 具 有 更 为 友善 的 人 机 界面 ， 未 来 希望 操作 人 员 只 需 给 出 要 完成 的 任务 ， 机 器 人 就 能 自主 完成 它 ; 另 一 方面 是 ， 提 高 智能 机 器 人 的 适应 性 ， 提 高 智能 机 器 人 适应 环境 变化 的 
能 力 ， 加 强 机 器 人 与 环境 之 间 的 交互 关系 。 


在 各 国 的 智能 机 器 人 发 展 中 ， 美 国 的 技术 在 国际 上 一 直 处 于 领先 地 位 ， 其 技术 全 面 、 先 进 ， 适 应 性 也 很 强 ， 性 能 可 靠 、 功 能 全 面 、 精 确 度 高 ， 其 视觉 、 触 党 等 人 工 智能 技术 早已 广泛 应 用 到 了 各 个 领 
域 。 日 本 由 于 一 系列 的 扶植 政策 ， 各 类 机 器 人 包括 智能 机 器 人 的 发 展 非常 迅速 。 欧 洲 各 国 在 智能 机 器 人 的 研究 和 应 用 方面 在 世界 上 也 处 于 公认 的 领先 地 位 。 我 国 起 步 较 晚 ， 而 后 进入 了 大 力 发 展 的 时 期 ， 以 
期 以 机 器 人 为 媒介 物 推动 整个 制造 业 的 改变 ， 从 而 推动 整个 高 技术 产业 的 壮大 。 


在 服务 机 器 人 领域 的 研发 方面 ， 我 国 与 日 本 、 美 国 等 国家 相 比 起 步 较 晚 ， 但 在 国家 863 计 划 的 支持 下 ， 我 国 在 服务 机 器 人 领域 已 开展 了 大 量 技术 研究 和 产品 开发 ， 并 取得 了 不 少 成 果 。2015 年 4 月 ， 我 国 
科技 部 正式 印发 了 《服务 机 器 人 科技 友 展 “十 三 五 ”专项 规划 》， 提 出 “十 三 五 ”服务 机 器 人 重点 专项 安排 公共 安全 机 器 人 、 仿 生机 器 人 平台 、 医 疗 康复 机 器 人 和 模块 化 核心 部 件 等 4 个 方面 的 任务 。 


2. 服 务 智能 机 器 人 的 分 类 
服务 型 机 器 人 按照 用 途 可 分 为 娱乐 服务 机 器 人 、 家 庭 服务 机 器 人 和 专业 服务 机 器 人 。 
(1) 娱乐 服务 机 器 人 


娱乐 服务 机 器 人 以 供 人 观赏 、 娱 乐 为 目的 ， 具 有 机 器 人 的 外 部 特征 ， 可 以 像 人 或 某 种 动物 一 样 ， 同 时 具有 机 器 人 的 功能 ,可 以 行走 或 完成 动作 ， 有 一 定 的 感知 能 力 、 语 言 能 力 和 交流 能 力 ， 如 机 器 人 歌 
手 、 足 球 机 器 人 、 玩 具 机 器 人 、 舞 蹈 机 器 人 等 。 


(2) 家 庭 服务 机 器 人 


家 庭 服务 机 器 人 用 于 在 家 庭 生活 中 承担 某 些 家 庭 事务 ， 如 照顾 孩子 学 习 玩 乐 ， 协 助 老人 更 衣 洗澡 ， 提 醒 病 人 按时 吃 药 ， 能 承担 诸如 端 条 倒 水 、 取 报纸 、 收 拾 餐具 、 倒 垃圾 、 吸 侍 、 探 窗户 等 家 务 活 ; 还 
可 以 进行 家 电 控制 、 防 盗 监 测 、 安 全 检查 、 接 听 电 话 等 ， 甚 至 可 以 操作 电脑 以 从 互联 网 上 收集 新 闻 及 其 他 信息 服务 等 。 依 据 具 体 的 服务 能 力 可 将 其 分 为 保姆 机 器 人 、 豪 饪 机 器 人 、 清 洁 机 器 人 、 保 安 机 器 
人 、 秘 书 机 器 人 、 护 理 机 器 人 等 。 此 类 服务 智能 机 器 人 具有 自主 路 径 规 划 、 自 动 障碍 回避 、 自 行 执行 操作 等 能 力 。 


(3) 专业 服务 机 器 人 


专业 服务 机 器 人 主要 用 在 特定 的 场所 ， 以 代替 人 完成 特殊 的 任务 。 它 主要 包括 导游 机 器 人 、 医 用 机 器 人 、 模 特 机 器 人 、 管 道 机 器 人 、 教 育 机 器 人 、 生 命 科学 机 器 人 、 建 筑 机 器 人 、 邮 拾 和 送信 机 器 人 及 
加 油 机 器 人 等 。 其 中 医用 机 器 人 又 可 以 分 为 手术 机 器 人 、 诊 断 机 器 人 、 护 理 机 器 人 、 康 复 机 器 人 和 助残 机 器 人 等 。 此 类 服务 智能 机 器 人 具有 适应 特殊 环境 、 自 主 完成 特定 操作 等 的 能 力 。 


8.1.2 ”服务 智能 机 器 人 的 关键 技术 

服务 智能 机 器 人 是 一 种 能 够 代替 人 从 事 多 类 工作 的 高 度 灵 活 的 自动 化 系统 ， 其 实现 是 集 力学 、 机 械 学 、 电 子 学 、 生 物 学 、 控 制 论 、 人 工 智能 、 系 统 工程 等 多 种 学 科 于 一 体 的 综合 性 技术 。 服 务 智能 机 器 
人 技术 在 本 质 上 与 所 有 其 他 类 型 的 智能 机 器 人 相似 ， 但 也 具有 自身 特点 。 

(1) 多 传感器 信息 融合 


多 传感器 信息 融合 技术 的 基本 原理 是 充分 地 利用 多 个 传 感 资 源 ， 通 过 对 各 种 传感器 及 其 观测 信息 的 合理 支配 与 使 用 ， 将 各 种 传感器 在 空间 和 时 间 上 的 互补 与 见 余 信 息 依据 某 种 优化 准则 组 合 在 一 起 ， 产 
生 对 观测 环境 的 一 致 性 解释 和 描述 。 


(2) 智能 实时 控制 
智能 实时 控制 可 有 效 地 利用 机 器 人 传感器 从 环境 中 获取 的 信息 ， 通 过 适应 性 实时 计算 和 智能 处 理 ， 自 动 获得 后 续 动 作 。 
(3) 自主 导航 与 定位 


在 服务 机 器 人 系统 中 ， 自 主导 航 是 一 项 核心 技术 ， 其 是 服务 机 器 人 研究 的 重点 和 难点 问题 。 自 主导 航 是 指 服 务 机 器 人 通过 传感器 感知 环境 信息 和 自身 状态 ， 实 时 识别 人 为 路 标 或 具体 实物 ， 实 现在 有 障 
或 无 障 的 环境 中 有 效 或 可 靠 地 到 达 它 的 目标 位 置 的 功能 。 


(4) 自动 路 径 规 划 


路 径 规划 是 指 在 服务 机 器 人 工作 空间 中 找到 一 条 从 起 始 状态 到 目标 状态 、 并 可 避 开 障碍 物 的 最 佳 路 径 。 


8.2 ” ROS 导航 功能 包 集 介绍 


ROS 导 航 功 能 包 集 主要 包含 导航 与 定位 、 路 径 规 划 等 功能 ， 这 也 是 服务 智能 机 器 人 在 开发 过 程 中 必 不 可 少 的 一 部 分 功能 。 


8.2.1 概述 


机 器 人 在 未 知 环境 中 需要 使 用 激光 传感器 (或 者 将 深度 传感器 转换 为 激光 数据 ) ， 先 进行 地 图 建 模 ， 然 后 根据 构建 的 地 图 进行 导航 与 定位 。 在 ROS 中 可 以 利用 以 下 三 个 功能 包 集 来 实现 自主 导航 。 
` Gmapping: 根据 激光 数据 (或 者 深度 数据 模拟 的 激光 数据 ) 构建 地 图 。 
. move_base: 根据 参照 消息 进行 路 径 规 划 ， 使 移动 机 器 人 到 达 指 定位 置 。 
. amcl; 根据 已 有 地 图 进行 定位 。 
整体 导航 包 的 格局 如 图 8-1 所 示 。 
"move base simple/goal " 
geometry msgs/PoseStamped 


— 


my " 
‘ante move base Hp a 


amcl ve 
fecovery_behavior> sensor sources 
sensor _msgs/LaserScan 
sensor_msgs/PointCloud 


| 
| 
"cmd_vel"geometry_msgs/Twsit 












no/ Al 
rome. am : /tf 
senor transforms 
tf/tfMessage internal 


nav_msgs/Path 


odometry source "odom" 
nav_msgs/Odometry 


base_controller 


图 8-1 导航 包 格 局 图 
其 中 椭圆 框 内 的 是 必须 使 用 的 基本 组 件 ， 虚 线 框 内 的 是 ROS 提 供 的 可 选 组 件 ， 圆 角 矩 形 框 提供 应 用 开发 者 需要 的 组 件 。 导 航 包 提供 的 主要 组 件 功能 及 作用 简 述 如 下 。 
(1) 传感器 坐标 转换 一 一 sensor transforms 


由 于 机 器 人 控制 中 心 不 一 定 是 在 传感器 上 ， 所 以 要 把 传感器 的 数据 转换 成 控制 中 心 上 的 坐标 信息 。 如 图 8-2a 所 示 ， 传 感 器 获取 的 数据 是 在 base_laser 的 坐标 系统 中 ， 但 控制 时 是 以 base_link 为 中 心 的 ， 
所 以 要 根据 两 者 的 位 置 关 系 进行 坐标 转换 。 变 换 过 程 只 需要 将 base_laser 和 base_link 两 者 之 间 的 位 置 关 系 告知 tf， 就 可 以 实现 自动 转换 。 其 中 关于 tf 的 具体 功能 原理 ， 前 面 章节 中 已 有 有 具体 论 壕 。base_laser 


与 base _link 之 间 的 坐标 转换 关系 如 图 8-2b 所 示 。 








base laser 
10cm 


-base _link mo 





a) 坐标 示意 图 


| eens asandan age aie ea = base link 
parent: tf i 
Aa (0.4.0.0.0.2) 
(0.3.0.0,0.0) 





base laser 





b) 坐标 转换 示意 图 


图 8-2 ”坐标 和 坐标 转换 示意 图 


(2) 机 器 人 导航 传感器 数据 输入 
一 般 只 


包 集 目前 仪 支持 激光 雷达 传感器 。 在 设计 过 程 中 ， 机 器 人 的 传感器 必须 使 用 sensor_msgs/Laserscan 或 sensor_msgs/PointCloud 的 格式 发 布 数据 。 
(3) 里 程 信息 一 一 odometry source 


里 程 信息 描述 的 是 机 器 人 相对 于 某 一 点 的 距离 ， 它 的 消息 类 型 结构 如 图 8-3 所 示 。 


只 有 两 种 数据 : 激光 传感器 数据 和 点 云 数据 。 机 器 人 可 以 使 用 多 个 传感器 感知 对 象 或 环境 的 变化 ， 一 般 情况 下 ， 可 以 编写 多 个 如 前 所 述 的 软 节点 来 处 理 这 些 数 据 并 实现 一 些 功 能 ,但 ROS 导 航 功 能 


std msgs/Header header 
ULNt32 seg 
time stamp 
string frame id 
string child frame ia 
geometry msgs/PoseWithCovariance pose 
geometry msgs/Pose pose 
geometry msgqs/Potint position 
float64 x 
Float64 y 
float64 z 
geometry msgqs/Quaternton orientation 
float64 x 
float64 y 
floató4 z 


float64 w 
float64[ 36| covariance 
geometry msgs/TwistWithCovartance twist 
geometry msgs/Twist twist 
geometry msgs/Vector3 Linear 
float64 x 


Tloat64 y 
Float64 z 
geometry msoqs/Vector3 angular 
float64 x 
f Loató4 y 
Tloat64 z 
float64[36] covariance 


如 图 8-3 所 示 ，nav_msgs/Odometry 提 供 了 机 器 人 从 frame id 坐标 系 到 child frame id 坐标 系 的 相对 位 置 ， 它 还 通过 geometry_ msgs/Pose 消 息 提 供 机 器 人 的 位 姿 信 息 ，geometry_ msgs/Twist 消 息 
表示 速度 信息 。 位 姿 信息 中 包含 着 两 个 数据 结构 ， 一 个 表示 欧 拉 坐 标 系 中 的 位 置 ， 另 一 个 则 使 用 四 元 向 量 描述 机 器 人 的 方向 ， 也 就 是 机 器 人 的 角 位 移 。 速 度 信息 包含 了 线 速度 和 角速度 两 个 结构 。 





(4) 基础 控制 器 一 一 base controller 
对 于 导航 功能 包 来 说 ， 基 础 控制 器 是 非常 重要 的 ， 因 为 这 是 唯一 能 够 有 效 控 制 机 器 人 的 方法 。 它 能 够 直接 和 机 器 人 的 电子 设备 进行 通信 。 


ROS 并 不 提供 任何 标准 的 基础 控制 器 ， 因 此 必须 自己 编写 针对 特定 机 器 人 平台 的 基础 控制 器 。 机 器 人 通过 geometry_msgs/Twist 类 型 的 消息 所 控制 。 这 一 类 型 正 是 在 上 述 Odometry 消 息 中 所 使 用 的 。 
基础 控制 器 必须 订阅 以 cmd_vel 为 名 称 的 主题 ， 且 生成 正确 的 线 速度 和 角速度 命令 以 驱动 机 器 人 。 


(5) 地 图 服务 一 一 map_server 
在 导航 过 程 中 ， 地 图 并 不 是 必需 的 ， 此 时 相当 于 是 在 一 个 无 限 大 的 空间 上 进行 导航 ， 并 且 没 有 任何 障碍 物 。 但 是 考虑 到 实际 情况 ， 在 具体 应 用 中 还 是 需要 一 张 地 图 的 。 


在 ROS 导 航 中 ，costmap_2d 包 主要 负责 根据 传感器 的 信息 建立 和 更 新 二 维 或 三 维 地 图 。costmap 采 用 网 格 (grid) 的 形式 ， 每 一 网 格 的 值 从 0 到 255， 分 为 三 种 状态 : 占用 (有 障碍 物 ) 、 空 闲 及 未 
知 。 


8.2.2 SCHUTT 


从 总 体 框架 图 (图 8-1) 中 可 以 看 出 ，move_base 提 供 了 ROS 导 航 的 配置 、 运 行 和 交互 接口 ， 它 主要 包括 如 下 两 个 部 分 。 
1) 全 局 路 径 规 划 (global planner) : 根据 给 定 的 目标 位 置 进行 总 体 路 径 的 规划 。 

2) 局 部 实时 规划 (local planner) : 根据 所 在 位 置 附 近 的 障碍 物 进行 千 避 规划 。 

1. 相 关 数 据 结构 


ROS 中 定义 了 MoveBaseActionGoa| 数 据 结构 存储 目标 位 置 数据 ， 其 中 最 重要 的 就 是 位 置 坐 标 (position) 和 方向 (orientation) ， 代 码 如 下 : 





move base msgs/MoveBaseActionGoal] : 
std msgs/Header header 
uint32 seq 





time stamp 
string frame id 
actionlib msgs/GoalID goal_id 
time stamp 
string id 
move base msgs/MoveBaseGoal goal 
geometry msgs/PoseStamped target pose 
std _msgs/Header header 
uint32 seq 
time stamp 
string frame id 
geometry msgs/Pose pose 
geometry msgs/Point position 
Floate4 x 
float64 y 





















































4 Z 
geometry msgs/Quaternion orientation 


4X 



































- y 
Float64 z 
] Ww 








2. 配 置 文件 


move_base 使 用 前 需要 配置 一 些 相关 参数 : 运行 代价 、 机 器 人 半径 、 目 标 位 置 距离 和 机 器 人 移动 速度 ， 这 些 参 数 包含 在 以 下 几 个 配置 文件 中 : 


base local planner Params .yaml 
costmap common params. yaml 
global costmap params. yaml 
local costmap params. yaml 











3. 全 局 路 径 规 划 

全 局 路 径 规划 在 navigation 中 实现 的 方法 是 基于 栅 格 地 图 的 cost 搜 索 ， 从 而 找到 最 优 解 ， 而 常见 的 A* 与 dijkstra 算 法 只 是 扩展 栅 格 路 径 的 方式 不 同 。dijkstra 一 般 消耗 时 间 和 空间 都 比 A* 多 ， 但 是 A* 找 到 
的 一 般 不 是 全 局 最 优 解 。 值 得 说 明 的 是 ，navigation 中 的 两 层 规划 之 间 的 联系 纽带 是 local_planner， 其 内 的 cost function 已 对 全 局 路 径 有 所 考虑 ， 也 就 是 说 local_planner 输 出 的 local path 会 靠近 global 
path， 而 具体 情形 与 当时 的 速度 及 障碍 物 情况 相关 。 

对 于 global costmap, global frame 的 选取 一 般 要 依据 机 器 人 的 需求 ， 如 果 是 在 已 知 地 图 中 ， 那 么 使 用 static map 预 生成 costmap 是 又 快 又 好 的 方法 。 使 用 map 坐 标 系 做 全 局 坐标 系 ， 假 如 要 执行 一 些 
探索 任务 ， 若 没有 先 验 地 图 ， 那 么 定位 数据 的 选取 则 取决 于 已 经 获取 了 什么 类 型 的 数据 。 同样 ，rolling_window 也 是 依据 需求 进行 选取 。 

4. 局 部 实时 规划 

局 部 实时 规划 是 利用 base_local_planner 包 来 实现 的 ， 该 包 使 用 Trajectory Rollout 和 Dynamic Window approaches 算 法 计算 机 器 人 每 个 周期 内 应 该 行驶 的 速度 和 角度 
(dx, dy, dtheta, velocties) 。 图 8-4 所 示 的 是 碰撞 示意 图 。 





图 8-4 碰撞 示意 图 


base local_planner 包 利用 地 图 数据 ， 通 过 算法 搜索 到 达 目 标的 多 条 路 径 ， 利 用 一 些 评价 标准 (是 否 会 撞击 障碍 物 ， 所 需要 的 时 间 等 ) 选取 最 优 的 路 径 ， 并 计算 所 需要 的 实时 速度 和 角度 。 其 
中 ，Trajectory Rollout 和 Dynamic window approaches 算 法 是 其 核心 ， 主 要 思路 具体 如 下 。 


1) 采样 机 器 人 当前 状态 。 

2) 针对 每 个 采样 速度 ， 计 算 机 器 人 以 该 速度 行驶 一 段 时 间 后 的 状态 ， 得 出 一 条 行驶 路 线 。 

3) 利用 评价 标准 为 多 条 路 线 打分 。 

4) 根据 打分 ， 选 择 最 优 路 径 。 

5) 重复 上 面 的 步骤 。 

在 理解 move_base 的 基础 上 ， 再 了 解 机 器 人 的 定位 与 导航 。gmaping 包 可 以 用 来 生成 地 图 ， 但 该 过 程 需要 使 用 激光 扫描 仪 或 深度 数据 。 

amc| 是 移动 机 器 人 二 维 环境 下 的 概率 定位 系统 。 它 实现 了 自 适应 蒙特 卡 洛 定位 方法 ， 可 以 使 用 己 有 地 图 和 粒子 滤波 器 跟踪 机 器 人 的 姿态 。 

ROS 中 的 自 适 应 蒙特 卡 洛 定位 算法 在 系统 中 有 很 多 配置 选项 。 这 些 选项 对 定位 算法 的 性 能 影响 较 大 ， 要 了 解 算法 的 具体 信息 ， 需 要 查看 amcl 文 件 。 


采用 自 适应 蒙特 卡 洛 定位 算法 的 节点 主要 使 用 激光 扫描 和 激光 雷达 地 图 。 当 然 也 可 以 通过 修改 代码 以 适应 其 他 类 型 的 传感器 数据 ， 如 声呐 或 双 目 视觉 系统 。 


8.3 ”基于 ROS 的 服务 智能 机 器 人 设计 案例 


8.3.1 ”先锋 3DX 机 器 人 一 一 利用 ROS 实 现 建 图 、 定 位 和 目 主 导航 


先锋 3DX 是 一 种 常用 的 科研 机 器 人 。 由 于 它 的 合适 尺寸 和 硬件 设备 的 平衡 结合 ， 特 别 适用 于 研究 室内 导航 。 先 锋 3DX 用 一 个 差分 驱动 控制 机 器 人 的 运动 ， 为 了 能 够 使 机 器 人 具有 建 图 、 定 位 的 能 力 ， 其 
安装 了 一 个 激光 测 距 仪 。 一 台 装 有 基于 Linux 内 核 的 ROs 电 脑 被 用 于 控制 机 器 人 ， 其 在 机 器 人 建 图 的 过 程 中 ， 通 过 操纵 杆 进行 远程 控制 。 激 光 传感器 和 先锋 机 器 人 均 通过 USB 端 口 与 电脑 相连 。 在 软件 层面 ， 


系统 使 用 了 ROS 多 个 功能 包 ， 包 括 Rosaria、sicktoolbox wrapper、gmapping、map server、amcl、move base、joy 及 rviz 等 。 以 上 这 些 功 能 包 均 提供 了 相应 的 功能 节点 。 


1 基于 


ROS 的 地 图 建立 过 程 


建 图 是 利用 机 器 人 上 的 传感器 数据 ， 建 立 其 周边 环境 空间 模型 的 过 程 ， 该 地 图 将 用 于 机 器 人 的 定位 和 导航 。 为 了 利用 ROS 建 立地 图 ， 需 要 在 Linux 系 统 中 执行 下 列 指令 ， 每 条 指令 包含 一 个 节点 及 它 所 在 


的 功能 包 : 


rosr 
rosr 
rosr 
rosr 
rosr 
rosr 
rosr 


区 


roscore 


un ROSARIA RosAria 

un joy joy node 

un teleop base teleop base 

un ROSARIA transform — 

un Sicktoolbox wrapper sicklms 








un mapping slam_gmapping 
un map server map saver -f mymap 


其 中 sicklms 节 点 需要 提供 正确 的 端口 参数 。transform 节 点 是 设计 者 自 建 的 一 个 节点 ， 用 于 坐标 系 转换 。 所 有 这 些 节点 的 运行 均 使 得 机 器 人 能 够 通过 操纵 杆 在 设 定 的 环境 中 移动 ， 并 且 创 建 地 图 。 先 锋 
机 器 人 电机 中 的 里 程 数 据 是 由 RosAria 节 点 提供 的 ， 而 激光 传感器 和 里 程 数据 在 建 图 过 程 中 被 slam_gmapping 节 点 使 用 ， 该 节点 利用 开源 的 SLAM 方 法 实现 。 在 扫描 了 整个 周围 的 环境 之 后 ， 它 会 创建 
mymap.pgm 与 mymap.yaml 文 件 。 其 中 pgm 文 件 是 地 图 的 图 像 ， 而 yaml 文 件 是 地 图 的 描述 信息 。yaml 文 件 将 在 后 述 的 定位 和 导航 中 得 到 应 用 。 图 8-5 显 示 了 建 图 过 程 所 获得 的 一 幅 地 图 。 
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图 8-5 ”实时 构建 的 地 图 











2. 利 用 ROS 的 定位 过 程 

定位 是 告知 机 器 人 在 环境 中 的 相对 位 置 。 定 位 一 般 基 于 里 程 计 、 激 光 仪 数据 和 一 幅 地 图 。 除 了 要 用 到 roscore、RosAria、transform、joy_node、teleop_base 和 sicklms 节 点 之 外 ， 下 面 这 些 节点 也 与 
机 器 人 定位 密切 相关 : 

$ roslaunch amcl amcl_ diff.launch 

$ rosrun map server map server mymap.yaml 

$ rosrun rviz rviz 





这 些 节 点 支持 实现 机 器 人 在 环境 中 确定 自身 位 置 。 功 能 包 amcl 中 的 节点 完成 定位 的 具体 功能 ， 它 不 仅 采 用 粒子 滤波 器 方法 ， 而 且 采 用 基于 KLD 采 样 的 样本 数量 自 适 应 计算 方法 。 执 行文 件 amcl.diff 是 从 
amcl 功 能 包 中 的 案例 文件 目录 开始 执行 ;定位 过 程 需要 map_server 节 点 以 获取 mymap.yaml 文 件 ; 节点 rviz 是 用 于 机 器 人 定位 的 可 视 化 。 图 8-6 显 示 了 机 器 人 在 地 图 中 的 自 定位 位 置 。 
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图 8-6 ”定位 








一 旦 建 图 和 定位 过 程 成 功 后 ， 利 用 功能 包 move_base 就 能 很 容易 地 实现 自主 导航 。 该 功能 包 利用 定位 信息 ， 提 供 机 器 人 移动 指令 ， 使 机 器 人 能 够 在 环境 中 安全 运动 ， 并 避免 与 固定 物品 或 移动 的 物体 发 
生 碰 撞 。 该 功能 包 也 为 局 部 规划 器 和 全 局 规划 器 保存 两 张 代价 地 图 。 图 8-6 也 展示 了 用 Rviz 工 具 显示 机 器 人 自主 导航 的 过 程 ， 在 依次 执行 各 节点 之 后 ， 表 执行 一 个 来 自 move_base 的 move_base.launch 文 


器 实现 路 径 规划 和 自主 导航 。Rviz 是 作为 交互 式 工具 使 用 的 ， 它 能 设置 机 器 人 的 初始 位 置 和 目标 位 置 。 
4. 实 验 验 证 
这 里 使 用 两 个 真实 环境 和 一 个 自 创 环境 进行 实验 验证 。 图 8-6 和 图 8-7 显 示 了 两 个 真实 动态 环境 的 地 图 。 
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图 8-7 真实 动态 环境 2 的 地 图 


图 8-8 是 人 为 设 定 的 环境 ， 是 为 了 测试 机 器 人 能 够 在 更 复杂 的 环境 中 完成 自主 定位 和 自主 导航 功能 。 
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图 8-8 自 定 义 人 造 环境 地 图 
5. 实 验 结论 


通过 上 述 实 验 ， 可 以 看 出 3DX 机 器 人 利用 ROS 能 够 快速 完成 建 图 、 定 位 和 导航 的 功能 ， 但 其 准确 度 会 受到 各 种 因素 的 影响 ， 包 括 环境 因素 和 机 器 人 自身 因素 。 例 如 ， 机 器 人 的 传感器 位 置 会 影响 数据 采 
集 的 准确 度 ， 从 而 影响 结果 的 准确 性 。 


8.3 ”基于 ROS 的 服务 智能 机 器 人 设计 案例 


8.3.1 ”先锋 3DX 机 器 人 一 一 利用 ROS 实 现 建 图 、 定 位 和 自主 导 般 


先锋 3DX 是 一 种 常用 的 科研 机 器 人 。 由 于 它 的 合适 尺寸 和 硬件 设备 的 平衡 结合 ， 特 别 适 用 于 人 研究 室内 导航 。 先 锋 3DX 用 一 个 差分 驱动 控制 机 器 人 的 运动 ， 为 了 能 够 使 机 器 人 具有 建 图 、 定 位 的 能 力 ， 其 
安装 了 一 个 激光 测 距 仪 。 一 台 装 有 基于 Linux 内 核 的 ROS 电 脑 被 用 于 控制 机 器 人 ， 其 在 机 器 人 建 图 的 过 程 中 ， 通 过 操纵 杆 进 行 远程 控制 。 激 光 传 感 器 和 先锋 机 器 人 均 通 过 USB 端 口 与 电脑 相连 。 在 软件 层面 ， 
系统 使 用 了 ROS 多 个 功能 包 ， 包 括 Rosaria、sicktoolbox wrapper、gmapping、map server、amcl、move_base、joy 及 rviz 等 。 以 上 这 些 功 能 包 均 提供 了 相应 的 功能 节点 。 


1. 基 于 ROS 的 地 图 建立 过 程 


建 图 是 利用 机 器 人 上 的 传感器 数据 ， 建 立 其 周边 环境 空间 模型 的 过 程 ， 该 地 图 将 用 于 机 器 人 的 定位 和 导航 。 为 了 利用 ROS 建 立地 图 ， 需 要 在 Linux 系 统 中 执行 下 列 指令 ， 每 条 指令 包含 一 个 节点 及 它 所 在 
的 功能 包 : 


roscore 

rosrun ROSARIA RosAria 

rosrun joy joy node 

rosrun teleop base teleop base 
rosrun ROSARIA transform 

rosrun sicktoolbox wrapper sicklms 
rosrun mapping slam gmapping 

rosrun map server map saver -f mymap 
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其 中 sicklms 节 点 需要 提供 正确 的 端口 参数 。transform 节 点 是 设计 者 自 建 的 一 个 节点 ， 用 于 坐标 系 转换 。 所 有 这 些 节点 的 运行 均 使 得 机 器 人 能 够 通过 操纵 杆 在 设 定 的 环境 中 移动 ， 并 且 创 建 地 图 。 先 锋 
机 器 人 电机 中 的 里 程 数 据 是 由 RosAria 节 点 提供 的 ， 而 激光 传感器 和 里 程 数据 在 建 图 过 程 中 被 slam_gmapping 节 点 使 用 ， 该 节点 利用 开源 的 SLAM 方 法 实现 。 在 扫描 了 整个 周围 的 环境 之 后 ， 它 会 创建 
mymap.pgm 与 mymap.yaml 文 件 。 其 中 pgm 文 件 是 地 图 的 图 像 ， 而 yaml 文 件 是 地 图 的 描述 信息 。yaml 文 件 将 在 后 述 的 定位 和 导航 中 得 到 应 用 。 图 8-5 显 示 了 建 图 过 程 所 获得 的 一 幅 地 图 。 
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图 8-5 ”实时 构建 的 地 图 
2. 利 用 ROS 的 定位 过 程 


定位 是 告知 机 器 人 在 环境 中 的 相对 位 置 。 定 位 一 般 基 于 里 程 计 、 激 光 仪 数据 和 一 幅 地 图 。 除 了 要 用 到 roscore、RosAria、transform、joy_node、teleop_base 和 sicklms 节 点 之 外 ， 下 面 这 些 节点 也 与 
机 器 人 定位 密切 相关 : 





$ roslaunch amcl amc]l diff.launch 
$ rosrun map server map server mymap.yaml 
$ rosrun rviz rviz 








这 些 节 点 支持 实现 机 器 人 在 环境 中 确定 自身 位 置 。 功 能 包 amcl 中 的 节点 完成 定位 的 具体 功能 ， 它 不 仅 采 用 粒子 滤波 器 方法 ， 而 且 采 用 基于 KLD 采 样 的 样本 数量 自 适 应 计算 方法 。 执 行文 件 amcl.diff 是 从 
amcl 功 能 包 中 的 案例 文件 目录 开始 执行 ;定位 过 程 需要 map_server 节 点 以 获取 mymap.yaml 文 件 ; 节点 rviz 是 用 于 机 器 人 定位 的 可 视 化 。 图 8-6 显 示 了 机 器 人 在 地 图 中 的 自 定位 位 置 。 
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图 8-6 ”定位 








一 旦 建 图 和 定位 过 程 成 功 后 ， 利 用 功能 包 move_base 就 能 很 容易 地 实现 自主 导航 。 该 功能 包 利用 定位 信息 ， 提 供 机 器 人 移动 指令 ， 使 机 器 人 能 够 在 环境 中 安全 运动 ， 并 避免 与 固定 物品 或 移动 的 物体 发 
生 碰 撞 。 该 功能 包 也 为 局 部 规划 器 和 全 局 规划 器 保存 两 张 代价 地 图 。 图 8-6 也 展示 了 用 Rviz 工 具 显示 机 器 人 自主 导航 的 过 程 ， 在 依次 执行 各 节点 之 后 ， 表 执行 一 个 来 自 move_base 的 move_base.launch 文 


器 实现 路 径 规划 和 自主 导航 。Rviz 是 作为 交互 式 工具 使 用 的 ， 它 能 设置 机 器 人 的 初始 位 置 和 目标 位 置 。 
4. 实 验 验 证 
这 里 使 用 两 个 真实 环境 和 一 个 自 创 环境 进行 实验 验证 。 图 8-6 和 图 8-7 显 示 了 两 个 真实 动态 环境 的 地 图 。 


Ermm 中 mn 





图 8-7 真实 动态 环境 2 的 地 图 


图 8-8 是 人 为 设 定 的 环境 ， 是 为 了 测试 机 器 人 能 够 在 更 复杂 的 环境 中 完成 自主 定位 和 自主 导航 功能 。 
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图 8-8 自 定 义 人 造 环境 地 图 


5. 实 验 结论 


通过 上 述 实 验 ， 可 以 看 出 3DX 机 器 人 利用 ROS 能 够 快速 完成 建 图 、 定 位 和 导航 的 功能 ， 但 其 准确 度 会 受到 各 种 因素 的 影响 ， 包 括 环境 因素 和 机 器 人 自身 因素 。 例 如 ， 机 器 人 的 传感器 位 置 会 影响 数据 采 
集 的 准确 度 ， 从 而 影响 结果 的 准确 性 。 


8.3.2 ”基于 ROS 的 足球 机 器 人 设计 (以 NAO 机 器 人 为 例 ) 


.需求 与 背景 分 析 


标准 平台 联赛 (SPL) 是 机 器 人 足球 联赛 的 缩写 ， 所 有 的 参赛 团队 使 用 相同 的 机 器 人 进行 竞争 。 自 从 2008 年 以 来 ， 使 用 的 均 是 Aldebaran NAO 机 器 人 。 不 同 的 参赛 团队 为 了 占有 优势 ， 开 始 研发 能 够 实 
现 机 器 人 全 自主 操作 的 控制 软件 。 但 各 个 控制 软件 之 间 很 少 有 代码 共享 ， 使 得 足球 机 器 人 控制 软件 发 展 较为 缓慢 。 其 中 一 个 特例 是 B_Human 队 伍 每 一 年 将 自 编 软件 代码 在 社区 共享 ， 但 其 他 团队 很 难 利用 其 
代码 ， 这 是 由 于 所 用 系统 相差 甚 远 造 成 的 。 同 样 ，B_Human 队 伍 也 很 难 直接 使 用 其 他 团队 的 软件 以 丰富 自身 。 


ROS 是 一 个 提供 元 操作 的 机 器 人 开源 软件 系统 ， 并 且 可 以 作为 一 个 共享 机 器 人 软件 的 框架 工具 。 除 了 能 够 提供 建立 机 器 人 应 用 的 合适 体系 和 利用 ROS 节 点 实现 不 同 软件 之 间 的 交流 与 通信 之 外 ，ROS 最 
大 的 吸引 力 体现 在 机 器 人 研究 协会 中 几乎 每 个 人 都 在 使 用 它 。 在 SPL 足 球 机 器 人 中 使 用 ROS 具 有 以 下 优点 。 


` 团队 之 间 能 够 更 容易 地 共享 软件 模块 。 

` 能 够 支持 团队 发 展 特 定 的 解决 方案 。 

` 增强 新 团队 在 联赛 中 的 相互 协作 。 

` 激励 不 同 团 队 的 机 器 人 控制 创新 (如 情感 控制 、 知 觉 控制 等 ) 。 

. 使 得 特殊 软件 模块 之 间 的 比较 和 标记 更 为 容易 。 

2. 基 于 ROS 的 足球 机 器 人 的 开发 过 程 

要 实现 ROS 在 足球 机 器 人 上 的 使 用 ， 需 要 完成 以 下 三 个 步骤 。 

第 一 步 : ROS 在 NAO 机 器 人 上 的 使 用 

ALbert-Ludwigs 大 学 的 类 人 机 器 人 实验 室 创建 了 一 个 nao-robot 功 能 包 集 ， 其 中 包含 了 整合 NAO 机 器 人 和 ROS 的 节点 。ROS 在 NAO 机 器 人 上 的 使 用 有 两 种 方式 。 


1) 交叉 编译 方式 : 在 宿主 机 上 开发 出 特定 的 系统 之 后 ， 将 编译 生成 的 文件 下 载 到 NAO 机 器 人 ，NAO 机 器 人 上 目前 使 用 的 一 般 是 NAOQI 嵌 入 式 操作 系统 。 以 下 论述 使 用 该 方式 在 NAO 机 器 人 上 应 用 
ROS, 


2) 远程 调用 方式 : 在 安装 有 ROS 的 电脑 上 ， 通 过 远程 操作 ， 控 制 NAO 机 器 人 。 在 这 种 情况 下 ， 需 要 在 机 器 人 的 NAOQI 系 统 上 装 有 支持 C++ 语言 和 Python 语言 的 SDK。 


第 二 步 : 整合 移植 B-Human 机 器 人 的 行走 和 移动 引擎 作为 一 个 ROS 节 点 


出 。 


型 。 


当前 ，UChile 机 器 人 使 用 B-Human 机 器 人 的 行走 和 移动 引擎 。 为 了 实现 该 移动 引擎 的 集成 ， 需 要 直接 利用 NAO 硬 件 接口 实现 共享 内 存 块 (NaoQi_DCM) 并 利用 读 写 移动 引擎 完成 平台 数据 的 输入 输 


要 将 B-Human 机 器 人 的 行走 和 移动 引擎 集成 为 一 个 ROS 节 点 ， 需 要 定义 一 些 类 型 的 消息 ， 以 便 保证 原始 B-human 机 器 人 的 通信 表达 方式 和 和 ROS 消息 之 间 的 兼容 性 。 表 8-1 中 展示 了 需要 定义 的 消息 类 


表 8-1 ”为 B-Human 移 动 节 点 定义 的 相关 的 ROS 消 息 类 型 


消息 名 称 


tloat pan 


HeadRequest( input) float tilt 


float speed 


speclalActionRequest 


Motion Request(input) walkRequest 


bikeRequest 


float[] angles 
FilteredJoimtDataPrev( output) int timeStamp 
float cycle Time 
float[] angles - 
FilteredJointData( output) int timeStamp - 
float cycle Time 
float[] data 
int] | currents 
| uint| | temperatures 
FilteredSensorData(output) ee 
uint timestamp 
unt usActuatorMode 
uint usTimesStamp 
bool contactSafe 
GroundContactState(output) bool contact 
bool noContactSafe 
float tilt 


float pan 
HeadJointRequest( output) 


bool reachable 


bool moving 


| 
JomntRequest( output) 

jointHardness 
OdometryData(output) 


Pose3D[] limbs - 
RobotM odel(output) Vector} centerOtMass - 


float totalMass 
TorsoMatrix(output) Pose3D offset 


具体 的 实现 过 程 如 下 所 示 。 





1) 移动 节点 订阅 相关 的 ROS 主 题 ， 等 待 一 个 动作 请 求 。 
2) 如 果 存 在 一 个 回调 函数 ， 那 么 输入 信息 也 要 被 复制 到 平台 上 。 
3) NaoQi-DCM 每 一 次 更 新 传感器 信息 时 ， 动 作 引擎 就 等 待 任务 ， 并 且 计 算 动作 引擎 获取 它 的 执行 器 要 求 和 里 程 计 信息 。 


4) 把 获取 的 输出 信息 发 给 平台 ， 同 时 发 布 到 与 它 相关 的 主题 之 上 。 











二 级 属性 


speclalActionType 
mode 

Pose2D speed 
Pose2D target 
bMotionType 

bool mirror 


filteredJointData 


int| | hardness 


5) NaoQi-DCM 获 得 执行 器 的 需求 值 后 ， 更 新 传感器 的 值 。 
第 三 步 : 利用 ROS 框 架 实 现 两 个 NAO 机 器 人 通信 


假定 机 器 人 1 有 UChile 机 器 人 团队 的 代码 ， 这 些 代码 能 够 实现 重要 信息 以 ROS 消 息 方 式 共 享 的 接口 。 这 些 信 息 包括 感知 器 信息 、 里 程 计 信息 及 机 器 人 位 置 等 。 机 器 人 2 只 有 一 个 B-Human 移 动 引 警 作为 
ROS 节 点 ， 它 没有 任何 视觉 或 感知 功能 。 


机 器 人 1 是 主 控 机 ， 知 道 球 在 场 上 的 位 置 ， 并 且 能 观察 到 球 。 机 器 人 1 将 它 看 到 球 的 全 局 位 置 发布 到 一 个 主题 上 ， 而 这 个 主题 被 机 器 人 2 订阅 。 机 器 人 2 知道 它 在 球场 上 的 初始 位 置 ， 收 到 一 个 来 自 机 器 人 
1 的 回调 信息 (包含 了 球 的 位 置信 息 ) 后 ， 然 后 将 其 转换 成 合适 的 导航 信息 (行走 要 求 ) ， 然 后 移 向 球 的 位 置 。 


机 器 人 1 把 球 的 位 置 分 享 给 机 器 人 2 具体 过 程 如 下 所 示 。 

1) 机 器 人 1 从 机 器 人 2 那里 得 到 一 个 ready 标 志 。 

2) 机 器 人 1 发 布 球 的 位 置 。 

3) 机 器 人 2 使 ready 标 志 无 效 ， 同 时 导航 移动 到 球 在 球场 的 位 置 。 
4) 一 旦 机 器 人 2 到 达 指 定位 置 ， 就 再 使 ready 标 志 生 效 。 

5) 返回 步骤 1。 


通过 以 上 三 大 步骤 ， 可 以 使 得 ROS 在 足球 机 器 人 上 得 到 广泛 的 应 用 。 由 于 ROS 具 有 开源 并 屏蔽 异 构 性 等 特点 ， 使 得 不 同 竞赛 团队 之 间 也 可 以 共享 各 自 的 代码 ， 从 而 推动 足球 机 器 人 控制 软件 研发 快速 的 
发 展 。 


8.3.3 ”基于 ROS 的 多 机 器 人 协作 AAL 体 系 染 构 


AAL 是 基于 环境 辅助 生活 (Ambient Assisted Living) 的 简称 ， 它 是 一 种 基于 信息 通信 技术 构建 的 老年 人 服务 系统 ， 可 以 为 老人 提供 生活 上 的 帮助 ， 使 老人 能 在 家 中 或 社区 更 好 的 活动 ， 以 提高 他 们 的 
生活 质量 ， 同 时 也 能 降低 陪伴 者 的 工作 压力 。 在 AAL 系 统 中 ， 较 为 重要 的 元 素 是 智能 轮椅 和 相关 的 服务 机 器 人 ， 如 何 有 效 地 控制 这 些 机 器 人 是 非常 关键 的 一 环 ， 在 实现 这 些 功能 的 同时 ， 降 低 其 成 本 也 是 
AAL 能 够 更 加 普及 的 重要 因素 。 


图 8-9 展 示 了 欧盟 考 拉 工 程 中 的 一 个 AAL 系 统 。 





图 8-9 基于 AAL 的 多 机 器 人 系统 


图 8-9 中 的 智能 家 庭 环境 由 两 部 分 组 成 : 静态 传感器 和 移动 机 器 人 。 其 中 家 具 设 备 上 有 RFID 标 签 、 温 度 传感器 、 照 相机 、 惯 性 测量 单元 等 。 这 些 传感器 之 间 通 过 基于 ZigBee 的 无 线 传感器 网 络 相 连 。 一 
个 服务 智能 机 器 人 ， 一 个 类 人 NAO 机 器 人 ， 还 有 一 个 电动 的 机 器 人 轮椅 协同 为 需要 的 人 提供 帮助 。 人 机 接口 (HMIs) 是 为 了 让 人 能 够 更 容易 地 访问 AAL 系 统 。 


上 述 系 统 可 以 实现 自主 导航 ， 多 种 形式 的 人 机 交互 ， 以 及 多 机 器 人 协同 控制 。 机 器 人 从 用 户 那 里 得 到 信息 ， 包 括 位 置信 息 和 服务 指令 。 例 如 ， 一 个 坐 在 轮椅 上 的 老年 人 想 喝 水 ,服务 机 器 人 接收 到 来 自 
轮椅 的 指令 和 当前 的 位 置信 息 ， 通 过 RFID 标 签 ， 服务 机 器 人 找到 放置 饮水 设备 的 位 置 ， 并 自动 送 到 人 手中 。 另 外 一 个 场景 是 当 轮 椅 很 难 通过 一 个 窜 门 或 走廊 时 ， 服 务 智能 机 器 人 可 以 在 前 面 充当 指 路 者 的 角 
色 ， 通 过 分 享 位 置信 息 和 指令 实现 自动 引导 。 由 于 拥有 这 些 特征 ，AAL 能 够 辅 肪 有 身体 缺陷 的 老人 的 日 常生 活 ， 并 能 够 降低 照顾 者 的 工作 压力 ， 减 少 他 们 的 精力 消耗 。 


AAL 体 系 的 机 器 人 控制 系统 也 是 基于 ROS 进 行 设计 开发 的 。 如 图 8-10 所 示 ， 自 主导 航 和 移动 控制 利用 ROS 提 供 的 功能 实现 ， 服 务 机 器 人 之 间 的 信息 和 需求 是 通过 主题 /服务 进行 相互 交换 的 。 
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图 8-10 基于 协作 机 器 人 系统 的 ROS 框 架 
消息 被 作为 本 体 定 义 ， 并 且 是 基于 XML 格式 的 定义 。 这 些 消息 和 ROS 内 核 消 息 是 完全 兼容 的 。 消 息 交 换 由 一 个 主机 节点 协调 控制 ， 每 一 个 机 器 人 和 主 节点 之 间 是 通过 无 线 Ad-hoc 网 络 连 接 在 一 起 的 。 


为 了 整合 人 机 接口 设备 以 便于 人 和 机 器 人 进行 交互 ， 系 统 在 ROS 中 设计 了 一 个 HMI 桥 节点 ， 该 节点 用 于 从 人 机 接口 设备 上 订阅 消息 和 发 布 指令 给 服务 机 器 人 完成 相应 的 动作 。 桥 节点 允许 用 户 触发 执行 
器 以 完成 自主 导航 和 有 关 动 作 ， 使 得 服务 机 器 人 能 够 适应 不 同 用 户 的 习惯 和 爱好 。 例 如 ， 一 个 照顾 者 能 够 用 手机 APP 发 送 一 条 短 消息 给 轮椅 初始 化 按摩 任务 。HMI 桥 是 一 个 C/S 架 构 的 程序 ， 主 要 功能 是 圭 
装 消 息 数据 类 型 和 结构 。 其 采用 语义 来 表示 环境 和 人 的 活动 。 例 如 ， 一 个 基于 语义 地 图 的 本 体 可 以 包含 环境 中 的 各 种 物体 ， 通 过 知识 的 处 理 和 推理 ， 用 户 能 用 自然 语言 去 告诉 机 器 人 自己 的 具体 意愿 。 


一 个 商用 的 电动 轮椅 六 有 嵌入 式 电脑 和 一 些 传感器 ， 包 括 声呐 传感器 、 激 光 扫 描 仪 、 惯 性 单元 、 光 学 编码 器 及 一 台 装 有 Linux 的 嵌入 式 PC 以 用 于 较 低 级 别 的 移动 控制 。 一 台 桌 面 PC 用 于 较 高 级 别 的 控 
制 ， 如 图 8-11 所 示 。 
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图 8-11 电动 轮椅 的 导航 系统 


服务 程序 运行 在 嵌入 式 PC 上 ， 输 出 传感器 数据 和 接收 来 自 桌 面 PC 的 指令 。 在 较 高 级 别 的 控制 中 ， 一 个 基于 ROS 的 系统 被 构建 并 且 能 够 提供 自主 导航 和 交互 接口 。 通 过 HMI 桥 ， 用 户 可 以 根据 他 们 的 喜 
好 利用 交互 接口 使 轮椅 执行 导航 功能 。 


由 于 ROS 已 经 为 NAO 机 器 人 和 SCITOS 机 器 人 提供 了 导航 和 控制 功能 节点 ， 这 里 主要 描述 ROS 为 轮椅 建立 导航 系统 的 过 程 。 
(1) 通信 过 程 


ROS 的 主题 /服务 机 制 通过 消息 提供 了 任务 请 求 和 响应 功能 。 服 务 端 程序 和 客户 端 程序 保证 了 外 部 PC 和 嵌入 式 PC 之 间 的 通信 。 在 服务 端 程序 中 定义 了 传感器 信息 和 与 执行 器 指令 统一 的 数据 类 型 和 数据 
结构 。 在 高 级 别 的 控制 平台 上 ， 客 户 端 程序 作为 一 个 消息 桥梁 ， 被 封装 到 ROS 导 航 包 中 并 作为 一 个 普通 的 节点 ， 命 名 为 Comm。 


(2) 轮椅 模型 和 坐标 转换 


ROSs 中 的 URDF 是 一 种 基于 XML 的 机 器 人 描述 格式 ， 并 且 该 描述 文件 能 在 仿真 器 Gazebo 和 可 视 化 工具 Rviz 中 被 解析 显示 。 根 据 轮 椅 的 实例 特点 ， 可 使 用 URDF 建 立 一 个 差分 驱动 模型 。tf 功 能 包 人 允许 用 
户 跟踪 随 着 时 间 变 化 的 多 个 坐标 系 ， 并 保证 在 不 同 坐标 系 之 间 坐 标点 的 转换 关系 。 使 用 执行 文件 可 以 对 tf 相关 参数 进行 配置 ， 包 括 轮 椅 基 座 连 接 、 激 光 仪 和 声呐 的 位 置 。 革 消息 可 在 节点 之 间 被 相互 订阅 和 发 
布 。 


(3) 基于 差分 驱动 的 移动 控制 


差分 驱动 包 (differential_drive) 提供 了 独立 于 电机 硬件 驱动 的 移动 控制 功能 。 它 从 导航 推 中 获取 速度 信息 并 将 电机 驱动 参数 发 布 出 去 。 同 时 ， 可 从 comm 节 点 中 获取 里 程 计 信息 ， 发 布 到 导航 堆 中 。 
在 差分 驱动 包 中 ， 可 使 用 PID 控 制 器 实现 轮椅 速度 的 控制 ，PID 参 数 可 在 执行 文件 中 进行 配置 。 


(4) 导航 包 配 置 


ROS 中 包含 一 系列 处 理 来 自传 感 器 数据 的 功能 包 ， 它 们 能 生成 正确 的 移动 指令 以 控制 服务 机 器 人 移动 到 目标 位 置 。slam_gmapping 提 供 了 基于 激光 仪 的 定位 和 建 图 功能 ， 以 获取 来 自 周围 的 传感器 数据 
并 建立 一 个 二 维 或 三 维 网 格 地 图 所 描述 的 环境 。map_server 能 够 把 地 图 信息 转换 为 ROS 服 务 并 且 利 用 传感器 数据 生成 地 图 文件 。amcl 提 供 了 自 适 应 蒙特 卡 洛 定位 方法 跟踪 机 器 人 在 周围 地 图 中 的 位 置 。 
nav_core 提 供 了 全 局 规划 和 局 部 规划 的 功能 ， 分 别 是 base_ global _planner 和 base local planner, base global_planner 使 用 封装 了 迪 杰 斯 特 拉 算 法 的 navfn ， 以 实现 导航 路 径 计算 。base local_planner 
在 ROS 中 使 用 Trajectory Rollout and Dynamic Window 方 法 实现 ， 其 将 产生 的 轨迹 和 移动 消息 传递 给 机 器 人 。 为 了 完成 导航 任务 ， 必 须 把 建 图 、 定 位 和 规划 器 集成 在 一 起 。ROS 中 的 move_base 扮 演 了 这 
么 一 个 角色 。 


总 的 来 说 ， 基 于 ROS， 多 机 器 人 架构 AAL 系 统 得 以 高 效 开 发 和 有效 集 成 ， 为 造福 老年 人 或 残疾 人 创造 了 条 件 。 


8.3.4 ”基于 ROS 的 助 老 服务 机 器 人 设计 


在 Nilsson 和 Brooks 分 别提 出 了 基于 规划 的 机 器 人 包容 体系 架构 之 后 ， 自 主机 器 人 控制 系统 框架 结构 设计 成 为 其 相关 研究 的 核心 问题 ， 在 近年 的 不 少 机 器 人 系统 研发 中 ， 研 究 者 们 提出 了 多 种 方法 用 于 
构建 机 器 人 的 多 层次 、 分 布 式 控制 架构 。 该 架构 主要 包括 符号 化 高 层 规划 层 和 封装 了 机 器 人 基本 作业 能 力 的 中 间 层 等 。 类 似 系统 中 较为 典型 的 有 Player、YARP、ROS、MIRA。 这 些 软件 架构 提供 了 典型 的 
人 机 交互 及 与 机 器 人 任务 执行 相关 的 软 构件 集 。 室 内 服务 机 器 人 系统 中 的 典型 代表 是 由 德国 Fraunhofer 公 司 从 1998 年 开始 研发 的 Care-O-bot。 它 配置 了 具有 万 向 轮 的 四 驱 移 动 平台 ， 同 时 装备 了 一 条 轻型 
机 器 臂 和 一 个 可 折 堵 的 托盘 ， 可 完成 物体 的 搬运 、 拾 取 和 放置 等 操作 。 该 机 器 人 的 原始 样机 是 基于 该 公司 自主 开发 的 集中 式 软件 架构 ， 目 前 已 经 转向 基于 ROS 的 分 布 式 软件 系统 进行 开发 。 


另 一 个 典型 的 室内 服务 机 器 人 PR2 采 用 了 万 向 轮 结构 移动 平台 ， 搭 载 可 升降 躯干 及 两 条 7 自由 度 机 械 臂 ， 可 升降 躯干 大 大 提高 了 该 机 器 人 的 工作 空间 。 同 时 该 机 器 人 软件 架构 完全 基于 ROS 开 发 。 许 多 研 
究 机 构 购买 了 PR2 作 为 科研 平台 ， 利 用 其 开发 的 软件 平台 进行 技术 成 果 的 交流 及 分 享 。 


1. 基 于 ROS 的 服务 智能 机 器 人 层次 化 软件 架构 


Robot-Era 项 目 软件 架构 通过 一 个 抽象 层 将 机 器 人 能 够 执行 的 任务 或 动作 封装 成 系统 中 的 服务”， 以 供 系统 内 其 他 节点 调用 。 大 量 分 布 式 的 ROS 节 点 提供 了 各 种 高 效 的 环境 感知 和 运动 规划 等 算法 ， 
并 通过 它们 完成 系统 中 的 大 部 分 实时 计算 与 运动 控制 。 


如 图 8-12 所 示 ， 机 器 人 软件 架构 可 抽象 为 4 个 层次 。 最 上 面 的 层次 包括 人 机 界面 (BS. ARE) 和 PEIS 接 口 。 其 中 PEIS 用 于 对 机 器 人 AAL 环 境 中 的 传感器 网 络 状 态 进行 监控 ， 并 对 各 种 物品 对 象 、 任 
务 进行 语义 信息 层次 的 管理 ， 同 时 还 提供 了 符号 化 的 多 机 器 人 任务 规划 器 用 于 控制 不 同 的 机 器 人 、 传 感 器 和 环境 中 的 其 他 智能 设备 。 
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图 8-12 ”室内 服务 器 软件 架构 


软件 架构 中 的 第 二 层 为 PElS-ROSs 中 间 层 ， 机 器 人 的 任务 执行 、 操 作 动 作 等 功能 被 封装 成 一 系列 的 “服务 ”， 作 为 PEIsS 与 ROS 之 间 的 接口 。 这 些 功 能 性 服务 分 为 3 个 级 别 : 基础 级 服务 (提供 单一 传感器 
或 执行 器 的 功能 ) 、 中 间 级 服务 (设计 多 个 传感器 及 执行 器 的 功能 ) 和 高 级 服务 (集成 基础 级 和 中 间 级 服务 的 相关 功能 ) 。 表 8-2 列 出 了 PEIS-ROS 中 间 层 中 提供 的 部 分 服务 示例 ， 图 8-13 展 示 了 在 传送 物品 
任务 中 间 层 的 响应 及 与 其 他 层 交 互 的 过 程 。 


表 8-2 室内 服务 机 器 人 的 部 分 服务 案例 


服务 
Emergency Stop 
GetCameralmage 
GetKinectImage 
GetLaserScan 
Movelo 
MovePtu 
RetractJacoArm 
DetectKnownObject 
GraspKnownObject 
PlaceObjectOnTray 
MoveHingedDoor 
DockToCoro 
HandOverObject 


DetectPerson 


WalkingSupport 


BringObject 


CleanTable 


CleanWindow 


级 别 


a. 


注 : LATIKAS, IKT FFRI, HATAR] 


描 


二 


安全 停止 所有 机 各 人 动作 

从 照相 机 发 送 照 片 到 PEIS 

获取 图 形 和 3D 点 云 数据 

为 导航 或 停 菲 读 取 激光 数据 

移动 机 人 右 人 到 给 定 的 位 置 

移动 机 天 人 头 部 到 给 定位 置 

移动 机 械 臂 到 给 定 的 停 徘 位 置 

找到 被 请 求 的 物体 ， 返 回 6 目 由 度 的 位 姿 信 息 

移动 手臂 到 物体 的 位 置 ， 并 抓 取 

把 抓 取 的 物体 放 在 机 器 人 的 托盘 上 

移动 手臂 去 抓 住 门 并 打开 

移动 到 机 器 人 共同 控制 的 位 置 ， 并 排 成 一 列 ， 便 于 物品 交换 

把 机 械 手 臂 移动 到 面 回 人 的 位 置 ， 接 受 语 音 指令 ， 然 后 放下 物品 
从 照相 机 图 片 中 识别 一 个 人 

明 用 户 的 方 同 移动 ， 不 断 地 旋转 直到 用 户 能 够 抓 住 操纵 需 ， 然 后 使 机 


信人 随 用 户 的 动作 而 移动 


实现 服务 DetectKnownObject、GraspKnownObject、PutObjectOnTray、 


DriverToUser 的 功能 


利用 HandOverObject 检测 果子 上 的 所 有 物品 ， 放 在 托盘 上 ， 珊 到 厨房 


去 ,然后 把 它们 放 在 厨房 的 水 池 里 


把 抹布 或 刷子 放 在 托盘 上 ， 回 窗户 移动 ， 实 现 一 个 清洁 的 动作 


中 间 层 提供 的 “服务 ”所 对 应 的 具体 功能 在 第 三 层 实现 ， 即 由 一 系列 节点 构成 的 ROs 系 统 层 。 其 中 ， 物 品 抓 取 / 搬 运 操作 将 基于 ROSs 中 提供 的 Movelt 框 架 来 实现 ， 该 框架 提供 了 基于 多 种 传感器 的 碰撞 
检测 和 基于 自 适 应 滤波 的 机 器 臂 运动 规划 等 。 设 计 者 通过 自行 开发 ROS 节 点 实现 jaco 机 械 辟 及 其 灵 I5 手 的 底层 控制 ， 另 外 构建 状态 监控 节点 ， 实 现 系统 服务 运行 时 序 管理 ， 并 将 各 级 任务 、 子 任务 执行 过 程 


中 的 信息 反馈 给 PEIS 及 人 机 界面 层 。 


软件 架构 层 的 第 四 层 为 系统 中 的 各 种 传感器 、 作 动 器 等 硬件 设备 的 软件 接口 。 其 中 MIRA 软 件 包 在 Scitos_G5 移 动 平 台 的 导航 定位 比 ROS 自 带 的 导航 定位 软件 包 具 有 更 优越 的 效果 ， 因 此 ，Robot Erai 
目 中 的 机 器 人 采用 该 软件 包 实 现 机 器 人 的 导航 定位 。 
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图 8-13 ”获取 物品 服务 的 运行 时 序 图 


机 器 人 的 仿真 模型 通过 URDF 文 件 描述 ， 包 括 机 器 人 的 几何 /运动 学 模型 、 各 种 传感器 的 安装 位 置 及 属性 参数 等 。 其 中 ROS 已 经 实现 了 对 大 部 分 典型 传感器 和 硬件 设备 的 软件 支持 ， 如 激光 雷达 、 
FireWare 报 像 头 和 XtionPro RGB-D 传 感 器 等 。 


2.PEIS-ROS 中 间 层 实现 


设计 PEIS-ROS 中 间 层 的 目的 是 为 了 对 机 器 人 能 够 提供 的 任务 执行 和 操作 能 力 进行 封装 ， 并 映射 到 PEIS 中 的 元 组 空间 中 。 元 组 空间 中 的 元 组 包括 对 任务 、 对 象 及 操作 状态 的 描述 ， 其 数据 均 包 含 一 个 语义 
标识 和 相对 应 的 数据 体 。PEIS 层 收 到 用 户 的 服务 指令 后 ， 提 取 任 务 及 对 象 的 语义 信息 实现 任务 规划 ， 以 生成 系统 中 不 同 机 器 人 所 执行 的 子 任务 (动作 序列 ) 。 


在 大 多 数 情况 下 ， 机 器 人 执行 动作 序列 的 过 程 会 持续 数 十 秒 至 几 分 钟 不 等 ， 因 此 设计 者 在 PEIS-ROS 中 间 层 采用 基于 动作 执行 过 程 状 态 反 馈 的 异步 服务 调用 策略 。 设 计 者 使 用 了 一 个 继承 自 actionlib 的 
C++ 类 exekutors 作 为 服务 调用 与 PE1S 的 接口 。 如 果 系 统 中 发 生 一 个 新 的 服务 请 求 ， 则 会 产生 一 个 新 的 exekutor， 其 将 通过 订阅 PEIS 网 络 中 特定 元 组 的 信息 对 当前 系统 中 所 执行 的 任务 状态 进行 监控 。 一 旦 
该 元 组 的 状态 发 生 改 变 ， 就 会 触发 相应 的 目标 函数 ， 开 始 执行 新 的 服务 及 任务 。 元 组 信息 中 的 数据 体 则 作为 目标 函数 的 参数 。 在 新 的 服务 及 任务 的 执行 过 程 中 ， 其 执行 状态 及 反馈 信息 通过 exekutor 实 时 反 
馈 给 PEIS， 直 到 执行 结束 。 


3. 目 标 物品 识别 与 抓 取 实现 


机 器 人 多 模 态 感知 设计 面向 室内 机 器 人 的 多 种 任务 场景 。 在 机 器 人 对 特定 物品 进行 抓 取 等 操作 时 ， 不 仅 需 要 准确 检测 到 目标 物品 ， 还 需要 获得 物品 与 机 器 人 的 相对 位 置 和 姿态 关系 。 为 此 ， 假 设 场景 涉 
及 的 物品 可 分 为 盒 状 物品 、 圆 柱状 物品 及 其 他 特殊 形态 物品 等 三 类 。 


设计 者 针对 上 述 不 同类 型 的 物品 感知 分 别 采 用 不 同 的 算法 ， 并 在 ROs 中 设计 了 Visionhub 节 点 实现 算法 整合 。 图 8-14 展 示 了 感知 系统 中 各 部 分 的 细节 及 其 整合 关系 。 图 8-15 展 示 了 在 一 个 典型 的 多 物品 
场景 中 物品 感知 的 结果 。 
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图 8-14 ”多 模 态 感知 系统 总 体 结构 
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图 8-15 ”随机 摆 放 的 桌面 物品 识别 


对 于 盒 状 物品 ， 由 于 其 6 个 表面 至 少 会 有 一 个 表面 完整 呈现 在 机 器 人 的 视野 中 ， 因 此 设计 者 采用 SIFT 算 法 实现 该 类 物品 的 检测 和 识别 。 在 机 器 人 的 数据 库 中 储存 有 目标 物品 6 个 表面 的 图 像 信息 及 准确 的 
几何 信息 。 机 器 人 通过 视觉 获 取 物 品 的 图 像 信息 ， 并 将 检测 到 的 物品 完整 表面 信息 与 数据 库 中 的 信息 进行 配 准 ， 从 而 识别 物品 并 计算 出 物品 空间 的 位 置 和 姿态 信息 。 


对 圆柱 形 物 品 的 检测 则 利用 了 XtionPro 摄 像 头 所 提供 的 3D 点 云 信息 。 由 于 物品 呈 旋 转 对 称 ， 物 品 的 姿态 信息 由 3D 退 化 成 2D， 从 而 导致 机 器 人 在 抓 取 姿态 的 选择 上 具有 一 定 的 随机 性 。 


作为 上 述 两 种 较为 常规 的 室内 物品 的 补充 ，Robot-Era 项 目的 场景 中 还 定义 了 另外 一 些 用 于 特殊 目的 的 盒子 或 篮子 类 物品 。 在 这 些 物品 上 均 设计 了 方便 Jaco 机 器 臂 进行 抓 取 的 把 手 ， 并 安装 了 AprilTag 
二 维 码 标签 用 于 对 物品 的 准确 识别 和 位 姿 计 算 。 这 样 ， 在 物品 识别 的 过 程 中 仅 依 靠 检 测 到 的 二 维 码 即 可 完成 物品 的 识别 和 定位 ， 而 不 需要 让 物品 全 部 进入 视野 。 通 过 对 二 维 码 的 识别 及 定位 ， 结 合 对 机 器 人 
手臂 抓 取 把 手 的 示 教 ， 计 算 获 得 把 手 与 二 维 码 的 相对 位 置 。 


在 任务 规划 和 执行 过 程 中 ， 上 述 三 种 物品 的 识别 及 定位 的 底层 算法 可 通过 Visionhub 节 点 进行 整合 ， 并 作为 高 层 任务 级 规划 过 程 中 唯一 物品 识别 结果 的 输出 节点 ， 与 操作 控制 、 任 务 规划 等 节点 共同 完 
成 机 器 人 的 物品 自主 抓 取 等 操作 的 规划 和 执行 。Visionhub 的 XML 配置 文件 中 包含 如 下 数据 。 


. 物品 包围 金 维度 。 
. 物体 表面 图 像 及 其 几何 尺寸 信息 。 
. 二 维 码 标签 及 几何 尺寸 、 位 置信 息 。 
. 抓 取 信息 (如 最 大 抓 握力 ) 。 


该 节点 还 提供 目标 物品 的 属性 、 功 能 及 抓 取 方 法 等 语义 信息 ， 用 于 作为 对 象 概念 化 构件 及 PEIS 中 语义 理解 构件 的 接口 。 


8.4 ”本 章 小 结 


本 章 在 介绍 服务 智能 机 器 人 特点 的 基础 上 ， 从 服务 智能 机 器 人 的 核心 技术 及 其 应 用 需求 出 发 ， 重 点 介绍 了 ROS 中 导航 功能 包 集 的 相关 原理 ， 基 于 ROS 的 服务 机 器 人 开发 与 应 用 的 4 个 具体 案例 。 
从 上 述 的 4 个 基于 ROS 的 服务 机 器 人 的 设计 中 可 以 看 出 ，ROS 对 服务 智能 机 器 人 的 主要 作用 体现 在 以 下 几 个 方面 。 

1) 利用 ROS 中 现 有 的 建 图 、 定 位 、 自 主导 航 功 能 ， 可 快速 实现 服务 机 器 人 的 基本 能 力 。 

2) 利用 ROS 的 主题 /服务 通信 机 制 可 方便 地 实现 多 机 器 人 协同 和 控制 操作 。 

3) 利用 ROS 的 仿真 和 可 视 化 工具 ， 可 对 具体 机 器 人 进行 仿真 和 模拟 ， 实 现 有 关 算 法 的 调试 和 测试 。 


我 们 认为 可 以 将 ROS 更 大 程度 地 应 用 到 服务 智能 机 器 人 设计 中 ， 在 机 器 人 上 直接 安装 ROS， 使 其 可 以 独立 自主 地 完成 相关 功能 ， 即 用 户 可 以 在 宿主 机 上 开发 完成 相应 的 程序 后 ， 直 接 导入 机 器 人 家 入 式 


计算 平台 中 独立 运行 。 


第 9 章 。” 基 于 ROS 的 工业 智能 机 器 人 设计 


9.1 工业 智能 机 器 人 及 其 软件 开 友 挑战 


9.1.1 工业 智能 机 器 人 


目前 应 用 于 工业 领域 的 工业 机 器 人 极 大 地 提高 了 劳动 生产 率 和 产品 质量 ， 降 低 了 成 本 和 人 力 的 劳动 强度 。 所 以 ， 世 界 上 的 发 达 国家 都 在 努力 发 展 工业 机 器 人 技术 及 其 工业 应 用 。 我 国 提出 的 《中 国 制造 


2025》 和 “十 三 五 ”发 展 规划 ， 均 将 工业 机 器 人 列 为 发 展 重点 。 工 业 机 器 人 是 先进 制造 技术 和 自动 化 装备 的 典型 代表 ， 其 涉及 机 械 、 电 子 、 自 动 控制 、 计 算 机 、 人 工 智 能 、 传 感 器 、 通 信和 与 网 络 等 多 个 学 科 
和 领域 ， 是 多 种 高 新 技术 发 展 成 果 的 综合 集成 。 


随 着 工业 机 器 人 在 制造 业 的 应 用 范围 越 来 越 广 阔 ， 不 仪 标准化、 模块 化 、 网 络 化 程度 越 来 越 高 ， 而 且 智 能 化 水 平和 特性 也 得 到 不 断 提升 ， 并 向 着 成 套 技术 和 装备 的 方向 发 展 ， 具 有 感知 能 力 并 能 对 感知 
言 息 作出 自主 反应 的 工业 机 器 人 称 为 工业 智能 机 器 人 ， 其 是 人 造 机 器 的 “终极 ”形式 ， 是 现代 工业 机 器 人 的 发 展 方向 。 


工业 智能 机 器 人 不 仅 应 配置 获取 外 部 环境 信息 的 各 种 拟人 化 传感器 ， 如 力 传感器 、 负 载 传感器 、 视 党 传感器 、 声 觉 传感器 等 ， 以 提高 工业 智能 机 器 人 对 周围 环境 的 自 适 应 能 力 ， 而 且 应 具有 行为 记忆 、 
语言 理解 、 图 像 识别 、 推 理 判 断 、 自 动 规划 及 自主 控制 、 人 机 协同 等 能 力 。 这 些 智 能 特性 不 仅 要 基于 微 电 子 和 机 电 一 体 化 技术 的 应 用 ， 而 且 更 多 地 依赖 于 软件 技术 的 有 力 支 持 。 
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言 息 作出 自主 反应 的 工业 机 器 人 称 为 工业 智能 机 器 人 ， 其 是 人 造 机 器 的 “终极 ”形式 ， 是 现代 工业 机 器 人 的 发 展 方向 。 


工业 智能 机 器 人 不 仅 应 配置 获取 外 部 环境 信息 的 各 种 拟人 化 传感器 ， 如 力 传感器 、 负 载 传感器 、 视 党 传感器 、 声 觉 传感器 等 ， 以 提高 工业 智能 机 器 人 对 周围 环境 的 自 适 应 能 力 ， 而 且 应 具有 行为 记忆 、 
语言 理解 、 图 像 识别 、 推 理 判 断 、 自 动 规划 及 自主 控制 、 人 机 协同 等 能 力 。 这 些 智 能 特性 不 仅 要 基于 微 电 子 和 机 电 一 体 化 技术 的 应 用 ， 而 且 更 多 地 依赖 于 软件 技术 的 有 力 支 持 。 


9.1.2 工业 智能 机 器 人 软件 开发 挑战 


目前 的 工业 机 器 人 软件 开发 大 多 是 各 个 研发 机 构 自行 开发 的 专用 封闭 系统 ， 虽 然 其 软件 与 硬件 的 结合 度 良好 ， 运 行 效率 也 较 高 ， 但 存在 代码 复 用 率 低 、 学 习 曲 线 长 、 不 支持 分 布 式 开 发 等 问题 ， 并 且 会 
导致 多 个 专用 系统 难以 交互 ， 机 器 人 软件 发 展 缓慢 。 例 如 ， 目 前 国内 主要 引进 的 Kuka、YASKAWA、ABB、Fanuc 等 工业 机 器 人 使 用 的 均 是 自己 的 控制 软件 系统 ， 这 就 导致 企业 若 要 维护 和 重新 编程 机 器 人 
会 很 困难 ， 更 换 机 器 人 种 类 需 重新 培训 人 员 ， 机 器 人 交互 界面 不 友好 等 问题 ， 从 而 降低 了 生产 效率 。 


如 前 所 述 ， 工 业 智能 机 器 人 的 “智能 ”特性 更 多 地 体现 在 算法 及 其 软件 上 ， 因 此 ， 工 业 智能 机 器 人 的 软件 结构 更 为 复杂 ， 功 能 更 加 丰富 ， 规 模 更 加 庞大 ， 行 为 更 加 动态 ， 传 统 的 封闭 结构 和 专用 的 开发 
方式 已 难以 适应 其 发 展 要 求 。 为 了 高 效 研发 工业 智能 机 器 人 软件 系统 ， 如 何 构建 统一 的 工业 机 器 人 软件 平台 ， 高 效率 研发 具有 智能 特性 的 工业 机 器 人 应 用 软件 ， 实 现 多 机 器 人 协同 及 人 机 协同 成 为 工业 智能 
机 器 人 软件 开发 面临 的 主要 技术 挑战 。 


9.2 ROS-Industrial 


9.2.1 ROS-Industrialfsj7} 


硬件 通用 化 、 软 件 平台 化 及 应 用 个 性 化 是 当前 智能 手机 的 主流 趋势 。 机 器 人 领域 也 有 人 在 进行 类 似 的 努力 : 有 没有 可 能 使 机 器 人 也 像 智 能 手机 一 样 ， 拥 有 通用 的 硬件 架构 、 统 一 的 软件 平台 ， 各 家 厂商 


的 系统 可 以 完全 兼容 呢 ” 为 此 机 器 人 操作 系统 ROs 应 运 而 生 ， 意 在 把 机 器 人 变 成 下 一 个 类 似 智能 手机 的 行业 ， 在 机 器 人 身上 实现 苹果 、 安 卓 曾经 的 平台 梦 。ROS 针 对 工业 机 器 人 专门 开发 了 一 套 系统 ROS- 
Industrial。 为 了 促进 其 推广 与 应 用 ，14 家 公司 成 了 了 ROS-Industrial 联 盟 (RIC) 。 经 过 不 断 发 展 ，ROS-Industrial 联 盟 现 已 规模 壮大 ， 包 括 美洲 联盟 和 欧洲 联盟 两 大 组 织 ， 此 外 还 有 众多 的 企业 也 参与 其 


中 ， 如 图 9-1 所 示 。 
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图 9-1 ROS-Industrial 联 盟 
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图 9-1 ROS-Industrial 联 盟 


9.2.2 ROS-Industrial 项 目 

ROS-Industrial (ROS-1) 开源 项 目 由 安 川 Motoman 机 器 人 、 美 国 西南 研究 所 和 Willow Garage 发 起 ， 意 在 实现 工业 自动 化 的 ROS 应 用 。ROS-Industria 拥 有 BSD/Apache2.0 证 书 ,项 目 包括 面向 工业 
硬件 的 库 、 工 具 和 驱动 接口 。ROS-Industrial 的 体系 结构 如 图 9-2 所 示 。ROS-Industrial 项 目的 目标 具体 如 下 。 

.将 ROS 的 优势 和 现 有 工业 技术 相 结合 ， 提 供 工业 相关 的 ROS 应 用 。 


开发 满足 工业 应 用 要 求 的 健 半 的 、 可 信 的 工业 软件 。 
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图 9-2 ”ROS-Industrial 体 系 结构 图 


> 采用 标准 化 ROS 消 息 创建 标准 接口 ， 激 励 硬 件 不 可 知 的 软件 开发 。 
. 通用 ROS 体 系 结构 使 前 沿 研 究 成 果 在 工业 应 用 上 简单 化 。 
ROS-Industrial 通 过 开源 设计 思想 ， 致 力 于 工业 应 用 ， 是 ROS 功 能 的 拓展 和 延伸 ， 其 优势 在 于 以 下 4 个 方面 。 
- 充分 发 挥 ROS 的 功能 性 ， 包 括 面向 控制 器 的 逆 运 动 学 解 算 、 强 大 的 2D 图 像 和 3D 点 云 感 知 ， 以 及 丰富 的 开发 、 仿 真 和 可 视 化 工具 集 。 
. 催生 新 的 应 用 ， 例 如 ， 机 器 人 工作 单元 的 识别 感知 等 非 结构 化 应 用 ， 基 于 感知 和 及 时 规划 的 Scan-N-Plan 算 法 ， 以 及 基于 模型 的 自动 化 编程 方式 等 。 
+ 简化 任务 级 的 机 器 人 编程 ， 包 括 自动 化 的 路 径 估算 、 冲 突 检 测 、 最 优 路 径 解 算 和 同 种 任务 抽象 化 编程 。 
" 节约 成 本 ， 开 源 软件 平台 使 得 商业 应 用 自由 化 ， 通 过 标准 化 机 器 人 和 传感器 接口 减少 开发 的 壁垒 。 


ROS-Industrial 项 目 仍 在 研发 中 ， 大 多 数 代码 仍 处 于 测试 阶段 ， 图 9-2 给 出 了 ROS-Industrial 软 件 框 架 的 具体 内 容 ， 包 括 应 用 层 的 GUI 设 计 、ROS 和 ROS-I 层 任务 划分 、ROS-I 接 口 屋 、ROS-I 简 单 消息 层 
及 ROS-I 的 控制 层 等 内 容 ， 具 体 如 表 9-1 所 示 。 


表 9-1 ROS-Industrial 各 层次 功能 描述 


ROS-| 层次 基本 搬 述 
- UI 分 为 两 个 部 分 ， 一 部 分 是 ROS 中 已 有 的 UI 工具 ， 男 外 一 部 分 是 
门 针 对 工业 机 需 人 的 通用 UI 工具 
ROS Layer ROS IER, TERE Li (a Lil 


ROS GUI 


ROS-| 层次 基本 摘 述 


Movelt Layer ALUM AL a AREER. ie a ED DD AE AY ETAT E 

ROS-I Application Layer 外 理工 业 生 产 的 具体 应 用 

ROS-I Interface Layer [ 业 机 器 人 人 的 客户 端 ， 可 以 通过 simple message PPI 5 BLAS AFE hl 4888 (A 
ROS-I Simple Message Layer | ROS 客户 唤 导 工业 机 古人 之 间 的 通信 协 以 ， 负 责 打 包 和 解析 通信 煞 气 
ROS-I Controller Layer lar A) tar ACA Dl Le A Pe tl at 


ROS-| 在 复 用 已 有 ROS 框 架 、 功 能 的 基础 上 ， 对 工业 领域 进行 了 针对 性 的 拓展 ， 而 且 可 以 通用 于 不 同 三 家 的 机 器 人 控制 器 。ROS-I 项 目 目前 主要 包含 以 下 几 部 分 的 内 容 。 
- 简单 消息 机 制 (Simple Message) 
“ 工业 机 器 人 客户 端 (Industry Robot Client) 
. 工业 标定 (Industrial Calibration) 
“ 具体 机 器 人 的 适 配 包 集 (Vendor Specific Stacks) 
1. 简 单 消息 机 制 
简单 消息 机 制定 义 了 ROS 驱 动 层 和 机 器 人 控制 器 本 身 通信 的 消息 结构 ， 协 议 遵 循 以 下 原则 。 
` 格式 简单 ， 使 得 代码 能 够 在 ROS 和 机 器 人 控制 器 (支持 C/C++) 之 间 共 享 。 对 于 一 些 不 支持 C/C++ 的 控制 器 而 言 ， 能 够 利用 能 力 有 限 的 标准 机 器 人 编程 语言 解析 消息 内 容 。 
+ 格式 支持 流 数据 模式 和 数据 反馈 。 
| 为 避免 不 同 通 信 平 台 之 间 存 在 版 本 冲突 的 问题 ， 协 议 不 包含 版 本 信息 。 
(1) 消息 结构 
消息 的 结构 如 表 9-2 所 示 。 


表 9-2 简单 消息 格式 


HEADER 通信 类 型 
BODY 可 变 长 度 ， 取 决 于 消息 和 通信 类 型 


(2) 类 型 化 消息 


消息 协议 允许 消息 包含 任意 的 数据 载荷 和 通信 类 型 。 但 是 ， 在 C/s 模 式 下 ， 通 信 双 方 需 要 知道 特定 的 消息 类 型 及 其 数据 载荷 。 类 型 化 消息 类 可 指定 数据 的 载荷 结构 。 类 型 化 消息 基 类 提供 了 创建 主题 、 
应 答 和 请 求 等 消息 方法 。 在 客户 端 和 服务 器 模式 下 ， 开 发 人 员 不 必 知 道 数据 载荷 的 格式 。 但 有 些 机 器 人 控制 器 并 不 支持 C+ + 类 ， 因 此 ， 开 发 人 员 必 须 理 解 消息 协议 及 载 倚 的 数据 结构 ， 以 便 能 够 在 机 器 人 控 
制 器 上 解析 消息 。 


关节 消息 : 表示 关节 的 位 置信 息 。 关 节 消 息 的 结构 如 表 9-3 所 示 。 


表 9-3 关节 消息 格式 


类 型 | E | 大 小 / 字 节 


F 


Pall 






消息 类 型 JOINT POSITION 4 
通信 类 型 
应 答 类 型 


和 f 


Shared _real[10] 任意 1 


关节 轨迹 点 消息 : 描述 关节 运动 轨迹 。 关 节 轨 迹 点 消息 的 结构 如 表 9-4 所 示 。 


表 9-4 关节 轨迹 点 消息 格式 


NES: 


Pall 





队列 


效 据 share read[10] 任意 
{ 





ir 
LI 
Lt 





share_real tH 
持续 时 间 


JointTrajectoryPoint 消 息 用 点 数据 表示 轨迹 ， 这 些 点 不 同 于 ROS 中 的 轨迹 点 ， 不 同 之 处 体现 在 以 下 几 个 方面 。 


i 


Ell 


“ 关节 的 速度 采用 工业 机 器 人 标准 。 


- 持续 时 间 也 不 同 于 ROS 中 时 间 锥 的 概念 。 时 间 惟 表示 运动 开始 的 时 间 ， 而 持续 时 间 表 示 的 是 动作 将 要 执行 的 时 间 长 度 。 此 外 ， 还 有 一 个 重要 的 前 提 是 队列 中 的 点 是 连续 被 执行 的 ， 这 一 点 比较 符合 


ROS 的 轨迹 特点 ， 但 并 不 是 必须 的 。 
(3) 通用 连接 
简单 消息 利用 抽象 连接 接口 向 工业 机 器 人 控制 器 发 送 消息 。 该 接口 有 两 个 前 提 假 设 : 首先 连接 支持 发 送 原始 的 字 节 数据 ， 同 时 数据 连接 包含 建立 连接 和 删除 连接 的 方法 。 
(4) 连接 管理 器 和 消息 处 理 器 


消息 管理 器 和 消息 处 理 器 支持 处 理 多 种 消息 类 型 的 连接 。 消 息 管理 器 包含 消息 处 理 器 的 列表 ， 当 收 到 消息 时 调用 相应 的 消息 处 理 器 进行 处 理 。 当 机 器 人 控制 器 仪 支持 有 限 个 连接 时 ， 这 些 类 就 显得 尤为 
重要 。 


2. 工 业 机 器 人 客户 端 

(1) 简介 

工业 机 器 人 的 客户 端 则 在 为 工业 机 器 人 提供 标准 的 控制 接口 ， 这 些 控 制 接口 遵循 ROS-I 的 设计 规范 ， 通 过 simple_message 协 议 实 现 与 运行 在 工业 机 器 人 控制 器 上 的 服务 器 通信 。 

最 主要 的 是 ，industrial_robot client 库 通过 C+ + 的 派生 类 机 制 实 现 具体 机 器 人 重用 该 库 中 的 代码 ， 从 而 避免 了 工业 机 器 人 驱动 产生 过 多 的 重复 设计 。 

同时 客户 端 也 提供 了 通用 的 节点 来 实现 基本 industrial_robot_client 功 能 。 当 工业 机 器 人 不 需要 客户 端 提供 额外 的 功能 时 ， 这 些 标准 节点 就 能 够 满足 客户 端 设计 的 需要 。 

(2) 面向 具体 机 器 人 的 改进 

通常 情况 下 ， 制 造 商 接口 和 机 器 人 设计 之 间 的 差异 使 得 industrial_robot _ client 的 基本 实现 需要 作出 一 定 的 修改 。 关 节 连 接 、 速 度 缩放 及 通信 协议 等 的 差异 都 是 影响 具体 机 器 人 控制 程序 设计 的 因素 。 

与 其 对 整个 ROS 客 户 端 做 一 个 新 备份 来 实现 机 器 人 客户 端 ， 不 如 采用 更 好 的 方法 : 使 用 C+ + 派生 类 机 制 ， 用 最 小 化 修改 来 实现 面向 特定 机 器 人 的 机 器 人 客户 端 。 但 是 无 论 以 什么 样 的 方式 实现 ， 控 制程 
序 都 应 该 调用 基 类 实现 的 功能 以 避免 代码 的 元 余 ， 并 维护 操作 的 一 臻 性。 同时 设计 人 员 需 要 非常 了 解 ROS 客 户 端 库 的 设计 思想 ， 以 便 能 够 合理 地 选择 和 蔡 换 某 些 功能 。 

3. 工 业 标定 


机 器 人 标定 是 离线 编程 技术 实用 化 的 关键 技术 之 一 ， 所 谓 标定 就 是 应 用 先进 的 测量 手段 或 几何 约束 等 方法 ， 采 用 基于 模型 的 参数 辨识 方法 辨识 出 准确 的 机 器 人 模型 参数 ， 从 而 提高 机 器 人 绝对 精度 的 过 
程 。 在 标定 过 程 中 ， 测 量 手 段 是 一 个 重要 的 因素 。 目 前 的 主要 方法 有 两 类 ， 一 是 采用 高 精度 测量 系统 进行 测量 ， 然 后 采用 数学 方法 进行 校正 ;二 是 机 器 人 自 校正 方法 ,具体 方法 是 在 末端 加 一 个 见 余 传感器 
收集 数据 ， 或 者 在 末端 加 一 些 运动 约束 ， 使 机 器 人 到 达 特 定位 置 。 

ROS 的 工业 标定 工具 箱 用 于 对 传感器 、 传 感 器 阵列 和 机 器 人 的 标定 。 目 前 正 处 于 开发 阶段 的 是 industrial_extrinsic_cal， 其 提供 通用 的 工具 对 已 知 参考 框架 的 传感器 进行 标定 ， 通 过 自动 运动 、 数 据 采 集 
和 程序 解 算 实 现 统 一 框架 下 精度 可 见 的 各 种 标定 任务 。 如 图 9-3 所 示 ， 利 用 目标 、 相 机 以 及 场景 的 具体 参数 信息 ， 通 过 代价 函数 计算 ， 生 成 校准 参数 。 


4. 具 体 机 器 人 适 配 


ROS-Industrial 包 含 对 多 种 工业 机 器 人 适 配 的 元 包 (meta-packages) ， 其 中 包括 ABB、Adept、Comau、Fanuc、Kuka、Motoman、Robotiq 及 Universal Robots 等 多 种 工业 机 器 人 。 但 是 大 多 数 
的 适 配 包 目 前 还 仅 适 用 于 实验 研发 ， 具 体 的 工业 应 用 仍 需 检验 其 代码 的 可 靠 性 。 






Set & Store 


Cameras.yaml 
Targets.yaml 
Scenes. yaml 
图 9-3 ROSindustrial_extrinsic_cal #&k A 4844 


LAMotoman (如 图 9-4) 为 例 ， 作 为 最 早 参 与 到 ROS-Industrial 项 目 中 的 工业 机 器 人 ， 目 前 其 适 配 包 仍 处 于 测试 阶段 。Motoman 驱 动 包含 与 Motoman 工 业 机 器 人 控制 器 通信 交互 的 节点 ， 旨 在 提供 
高 性 能 接口 控制 Motoman 机 器 人 ， 该 接口 在 以 下 多 个 方面 均 做 了 改进 。 


[机 器 人 控制 器 
mess 





| 开源， 由 ROS 社 区 支持 和 维护 
| 开源 ， 由 ROS 和 Yaskawa Motoman 陪 合 支持 和 维护 





图 9-4 Motoman stack 体 系 结 构 


- DX100、DX200 和 FS100 控 制 器 的 机 器 人 提速 一 倍 。 
- 增加 底层 位 置 、 速 度 和 路 径 时 序 控制 。 

+ 提高 错误 监测 和 恢复 性 能 。 

+ 提高 运动 和 控制 器 状态 反馈 性 能 。 


如 图 9-4 所 示 ，Motoman 驱 动 通 过 简单 的 消息 机 制 与 ROS 通 信 ， 同 时 也 包含 特定 的 Motoman 消 息 。 上 层 应 用 规划 的 轨迹 以 主题 的 形式 发 送 给 控制 器 节点 ， 其 主要 是 ROS 的 JointTrajectoryPoint 数 据 ， 
包括 关节 位 置 、 速 度 、 加 速度 和 路 径 时 序 。 控 制 器 节点 将 缓存 这 些 数 据 并 进行 处 理 ， 在 合适 的 时 机 向 MotoROS 控 制 器 发 送 控 制 指令 。 


Motoman 驱 动 接口 为 Motoman 机 器 人 提供 了 一 个 高 性 能 的 运动 控制 接口 。 其 发 送 数 据点 频率 超过 50Hz， 同 时 支持 机 器 人 的 状态 反馈 。 即 使 使 用 稠密 的 轨迹 点 ， 也 能 够 生成 全 速 的 机 器 人 运动 。 这 样 
的 性 能 甚至 已 经 超过 了 Motoman 原 生 的 运动 控制 机 制 。 


5.ROS-Industrial 驱 动 设计 规范 


工业 机 器 人 客户 端 实 现 了 与 支持 以 太 网 套 接 字 的 工业 机 器 人 控制 器 的 通信 ， 客 户 端 节 点 实际 上 是 作为 遵循 简单 消息 协议 机 器 人 的 驱动 程序 。ROS-Industrial 驱 动 设计 规范 明确 了 ROS 节 点 的 功能 性 设计 
要 求 ， 以 此 来 提高 跨 平 台 的 兼容 性 ， 所 有 面向 工业 机 器 人 控制 器 的 接口 都 应 当 遵 循 该 规范 。 


(1) 设计 初衷 


ROS-I 利 用 统一 的 ROS 框 架 集成 不 同 的 机 器 人 控制 ， 以 此 来 实现 不 同 供应 商机 器 人 之 间 的 互 操作 。 不 同 的 ROS 节 点 遵循 共同 的 控制 和 反馈 接口 来 实现 更 好 的 兼容 性 。 该 规范 仪 仪 是 提供 设计 参考 ， 指 明 
一 般 工 业 机 器 人 应 提供 怎样 的 接口 才能 实现 最 大 化 的 兼容 性 ， 但 在 实际 应 用 中 ， 具 体 的 机 器 人 可 能 会 根据 其 特殊 需要 在 某 些 方面 做 适度 地 修改 。 


(2) 操作 定义 

“ 初始 化 

ROS 节 点 会 自动 完成 对 机 器 人 控制 器 连接 的 初始 化 (不 需要 手动 的 操作 ) ， 同 时 机 器 人 控制 器 启动 时 ， 机 器 人 端 应 自动 执行 相应 的 控制 程序 。 
* 通信 


第 一 ， 无 论 是 ROS 闻 的 节点 还 是 机 器 人 端的 节点 ， 双 方 都 能 够 处 理 通 信 中 断 的 情况 。ROS 端 节点 在 出 现 通 信 中 断 的 情况 下 ， 应 能 自动 地 尝试 重新 连接 机 器 人 ， 停 止 发 布 大 部 分 消息 ， 同 时 发 布 诸如 
connect=false 的 状态 消息 。 而 机 器 人 端 节点 应 该 能 够 停止 运动 ， 关 闭 驱 动 程序 ， 重 新 初始 化 所 有 的 通信 连接 ， 等 待 建立 新 连接 。 如 果 接 口 不 能 直接 检测 通信 中 断 ， 那 么 应 在 ROs/Vrobot 之 间 应 用 心跳 机 
制 ， 以 此 来 判断 双方 通信 是 否 中 断 。 


第 二 ， 无 需 任 何 机 制 来 确保 异常 事件 出 现 ， 这 就 要 求 任何 节点 都 不 能 订阅 和 发 布 不 支持 的 主题 ， 同 时 节点 提供 的 服务 一 旦 被 广播 就 认为 是 有 效 的 ， 一 旦 节点 调用 未 实现 的 服务 ， 标 准 的 ROSs 机 制 就 应 该 


给 出 错误 的 提示 。 
(3) ROS API 
主要 描述 具体 机 器 人 接口 所 包含 的 主题 、 服 务 和 参数 。 这 些 功能 可 能 由 单个 或 多 个 节点 参与 ， 具 体 还 需要 根据 机 器 人 的 通信 架构 来 确定 。 
. 参数 
机 器 人 IP 地 址 (String) : 用 于 机 器 人 的 连接 。 


机 器 人 摘 述 (URDF) : 用 于 机 器 人 的 描述 。 


若 要 查看 机 器 人 状态 ， 则 需要 通过 订阅 相关 主题 ， 其 中 涉及 的 相关 主题 主要 有 Feedback states, Joint states 和 Robot status, Feedback states 提 供 机 器 人 关节 当前 的 状态 反馈 ， 一 般 由 ROS-| 关 节 
轨迹 动作 节点 来 监视 正在 执行 动作 的 执行 状况 。Joint_states 提 供 机 器 人 关节 当前 的 位 置 反馈 ， 由 机 器 人 状态 发 布 节点 广播 运动 学 变换 信息 。Robot_ status 提供 机 器 人 当前 状态 的 关键 参数 ， 用 于 其 他 节点 的 
监控 及 触发 不 同 的 错误 执行 条 件 。 


“ 运动 控制 


该 节点 主要 实现 对 机 器 人 的 运动 控制 ， 但 应 用 不 同 的 控制 类 型 可 能 会 导致 彼此 间 的 不 兼容 。 机 器 人 通过 单一 的 “master motion node” 或 功能 单一 的 节点 提供 相应 的 控制 方法 。 来 自 不 同 运 动 控制 器 
的 冲突 输入 能 否 被 妥善 处 理 需要 依赖 于 其 实现 机 制 。 一 般 而 言 ， 在 同一 时 刻 只 有 一 个 控制 器 能 够 控制 机 器 人 。 对 于 一 个 复杂 的 系统 ， 可 以 通过 一 个 动作 管理 节点 实现 不 同 动作 控制 器 的 竞争 指令 聚合 和 优先 


为 实现 实时 的 任务 规划 和 避免 冲突 ， 节 点 应 该 提供 具体 机 器 人 的 逆 运 动 学 解 算 。ROS 虽 提供 了 通用 的 解决 方案 ,但 其 实现 方式 对 冲突 检测 和 路 径 规划 响应 较 慢 。Ikfast 工 具 可 以 用 来 生成 任意 的 六 自由 度 
操控 器 所 需 的 运动 学 文件 ， 为 了 避免 过 多 的 通信 开销 ， 该 文件 将 作为 插件 集成 到 ROS， 而 不 是 作为 一 种 服务 ， 这 就 需要 创建 一 个 launch 文 件 ， 其 通过 ROS 中 的 arm_kinematics_constraint_aware 与 插件 建 


TER, 
. 轨迹 精度 
以 上 所 述 的 运动 控制 接口 并 没有 指明 具体 轨迹 的 精度 要 求 。 实 现 轨迹 精度 取决 于 多 方面 的 限制 ， 包 括 具体 的 机 器 人 、 控 制 器 及 ROS 接 口 驱动 等 


ROS 轨 迹 规划 器 和 碰撞 检测 器 在 轨迹 和 路 径 点 之 间 采 用 高 阶 平滑 的 方法 。 作 为 机 器 人 执行 的 最 终 轨迹 ， 应 该 是 近似 光滑 的 轨迹 但 和 理想 轨迹 并 不 完全 一 致 。 所 以 ROS 轨 人 迹 规划 器 在 机 器 人 模型 中 需要 增 
加 一 定数 量 的 插 补 点 来 弥补 规划 路 径 和 实际 路 径 间 的 差异 。 增 加 插 补 会 减少 碰撞 的 概率 ， 这 样 就 能 在 很 大 程度 上 保证 无 碰撞 的 运动 。 


路 径 规 划 器 和 机 器 人 控制 器 之 间 的 差异 越 小 ， 路 径 执 行 错误 和 冲突 揪 补 的 需要 也 就 越 小 。 但 该 层面 的 集成 已 经 超越 了 当前 ROS-Industrial 工 作 预 期 的 范围 ， 同 时 也 需要 基于 具体 的 机 器 人 解决 方案 。 


9.3 ”基于 ROS-Industrial 的 工业 智能 机 器 人 开发 实例 


针对 智能 工厂 自动 化 应 用 的 需求 ，ROSs-Industrial 主 要 致力 于 包括 非 固定 的 自动 化 、 动 态 分 拣 、 柔 性 自动 化 及 传感器 自动 化 等 方面 ， 面 向 其 提供 可 靠 的 自动 化 技术 支持 ， 但 是 当前 这 些 应 用 场景 的 ROS- 
Industrial 实 现 还 处 于 研发 阶段 ， 实 际 应 用 还 需要 进一步 开发 和 实践 。 


9.3.1 基于 ROS-Industrial 的 智能 喷涂 机 器 人 设计 


1. 自 动 化 喷涂 技术 简介 


在 现代 化 工厂 中 ， 自 动 化 喷涂 技术 已 经 得 到 了 较为 广泛 的 应 用 。 与 传统 的 机 械 喷 涂 相 比 ， 采 用 喷涂 机 器 人 可 大 大 降低 人 工 喷 涂 的 劳动 强度 ， 解 决 人 工 喷涂 质量 不 稳定 的 问题 ， 不 仅 能 节约 人 力 成 本 ， 而 
且 还 能 提高 喷涂 质量 与 效率 。 同 时 由 于 柔性 机 器 人 的 安装 位 置 比较 灵活 ， 例 如 ， 可 以 安装 在 地 面 、 倒 立 悬 挂 在 喷漆 室 顶部 和 侧面 ， 能 够 针对 喷漆 对 象 的 形状 与 大 小 灵活 喷漆 。 不 仅 如 此 ， 机 器 人 由 于 喷涂 的 
稳定 性 和 一 致 性 ， 不 会 出 现 超 范围 喷涂 ， 从 而 可 以 大 幅度 节约 油漆 ， 提 高 油漆 的 回收 率 。 安 全 是 喷漆 环节 非常 重要 的 因素 ， 智 能 喷涂 机 器 人 的 主要 贡献 之 一 就 是 将 喷涂 操作 者 从 危险 的 工作 环境 中 替换 出 


zt 


o 


自动 化 喷漆 机 器 人 是 可 以 进行 自动 喷漆 或 喷涂 其 他 涂料 的 工业 机 器 人 。 喷 漆 机 器 人 主要 由 机 器 人 本 体 、 计 算 机 和 相应 的 控制 系统 组 成 。 多 采用 5 或 6 自由 度 关 节 式 结构 ， 手 壁 有 较 大 的 运动 空间 ， 并 可 进 
行 复杂 的 轨迹 运动 ， 其 腕 部 一 般 有 2 ~ 3 个 自由 度 ， 可 灵活 运动 。 较 先进 的 喷漆 机 器 人 采用 柔性 腕 部 ， 既 可 向 各 个 方向 弯曲 ， 又 可 转动 ， 其 动作 类 似 于 人 的 手腕 ， 能 方便 地 通过 较 小 的 孔 伸 入 工件 内 部 ， 喷 涂 
其 内 表面 。 


2. 基 于 ROS 的 智能 喷涂 机 器 人 开发 


ROS 与 喷涂 机 器 人 相 结合 ， 既 能 够 充分 利用 ROS 的 标准 接口 和 代码 重用 性 以 减少 集成 成 本 ， 提 高 智能 喷涂 机 器 人 的 软件 开发 效率 与 质量 ， 又 能 够 发 挥 喷涂 机 器 人 的 优势 ， 即 能 够 在 降低 成 本 、 减 少 原料 
和 提供 智能 产品 的 一 致 性 方面 带 来 的 好 处 。 但 其 开发 需要 解决 无 约束 工件 位 置 、 随 机 实时 处 理 及 运动 部 件 协调 等 具体 问题 。 上 述 问题 的 有 效 解决 需要 依赖 于 ROS 相 应 的 技术 支持 。 


例如 ， 在 自动 化 喷涂 场景 中 ， 包 括 3D 感 知 、3D 数 据 处 理 、 路 径 规 划 、 机 器 人 逆 运 动 学 解 算 、 机 器 人 工作 空间 可 视 化 及 数据 采集 和 重 放 等 不 同 技术 。ROS 具 体 功 能 应 用 方式 简 述 如 下 。 


- OpenNI (Open Natural Interaction) : 即 开放 式 自然 操作 ， 其 中 定义 了 包括 语音 、 手 势 、 身 体 动作 等 基本 操作 方式 ， 同 时 也 定义 了 编写 自然 操作 程序 所 需要 的 API， 提 供 了 多 语言 跨 平 台 的 框架 和 标准 
接口 ， 能 够 让 开发 者 使 用 高 视觉 、 上 声音 相关 等 的 交互 传感器 ， 利 用 ROS 集 成 的 第 三 方 DpenNI 模 块 ， 可 以 实现 喷涂 过 程 的 3D 数 据 获 取 ， 例 如 具体 工件 尺寸 ， 位 置 、 实 时 场景 定位 及 喷涂 机 器 人 的 位 姿 等 关键 数 
据 ，OpenNI 开 源 的 特性 使 得 开发 人 员 能 够 快速 地 把 所 需 的 硬件 集成 到 系统 中 ， 从 而 实现 系统 的 3D 感 知 功能 。 


PCL (Point Cloud Library) : 用 于 2D/3D 图 像 和 点 云 处 理 与 开发 ， 包 括 过 滤 、 特 征 估 计 、 表 面 重建 、 注 册 、 模 型 适 配 和 分 隔 等 多 种 算法 。 这 些 算法 能 够 用 于 嗓 声 滤波 、3D 点 云 拼接 、 场 景 分 隔 、 抽 取 
关键 点 计算 和 识别 物体 的 描述 符 等 。PCL 强 大 的 3D 数 据 处 理 能 力 ， 能 够 快速 地 处 理 喷 涂 应 用 的 3D 数 据 ， 能 够 提供 机 器 人 对 场景 的 辨识 和 认 知 能 力 ， 是 喷涂 机 器 人 进行 自主 行为 优化 并 可 靠 完成 所 承担 任务 的 
前 提 和 基础 。 


ws 


* Movelt: 是 ROS 非 常 重要 的 模块 ， 同 时 也 是 ROS 控 制 机 器 人 运动 的 核心 组 件 。 其 中 Setup Assistant 向 导 工 具 用 于 配置 MoveIt， 运 动 学 模块 通过 对 URDF 的 文件 解析 生成 机 器 人 的 运动 学 求解 方 
法 ，KDL (kinematics dynamics library) 可 实现 对 机 器 人 遂 运 动 学 的 求解 ， 求 解 过 程 是 基于 数值 计算 的 方法 。 在 喷涂 应 用 中 ， 喷 涂 机 器 人 智能 特性 的 前 提 就 是 MoveIt 的 运动 规划 能 力 ， 喷 涂 应 用 中 ， 机 械 辟 的 


终端 位 置 通常 是 执行 的 位 置 ， 但 是 具体 关节 的 移动 路 径 就 需要 MoveIt 的 逆 运 动 学 解 算 ， 同 时 需要 规划 出 与 具体 场景 无 冲突 的 运动 规划 方案 。 


-Rviz: 是 ROS 中 功能 强大 的 3D 可 视 化 工具 ， 可 以 显示 创建 的 机 器 人 3D 模 型 ， 创 建 地 图 及 3D 点 云 的 显示 等 ，Gazebo 是 ROS 中 的 物理 仿真 环境 ，Gazebo 本 身 就 是 一 款 机 器 人 的 仿真 软件 ， 喷 涂 应 用 场景 
中 ， 实 现场 景 的 可 视 化 对 于 场景 的 实时 监控 非常 重要 ，ROS 提 供 了 Rviz 和 Gazebo 仿 真 软件 ， 能 够 实现 喷涂 场景 中 喷涂 机 器 人 工作 空间 的 可 视 化 。 


-rosbag: 是 ROS 主 题 消息 记录 和 重 放 的 集成 工具 ， 具 有 命令 行 模 式 和 API 两 种 使 用 形式 。ros/record 用 于 记录 系统 中 发 布 的 主题 ， 可 以 存放 在 特定 的 bag 文 件 中 ，ros/play 用 于 检验 和 重 放 bag 文 件 。rosbag 可 
用 于 记录 喷涂 过 程 中 的 数据 ， 这 些 数据 存放 在 特定 的 文件 中 ， 可 作为 生产 过 程 的 日 志文 件 ， 在 系统 发 生 故 障 时 ， 能 够 实现 快速 的 检查 和 维护 。 


通过 上 述 ROS 关 键 功能 对 喷涂 应 用 需求 的 支持 ， 喷 涂 场 景 能 够 充分 利用 ROS 模 块 化 的 优势 ， 快 速 地 集成 和 构建 出 其 原型 系统 ， 但 是 现 有 的 ROS 及 ROS-Industrial 其 可 靠 性 和 实时 性 还 不 能 满足 真实 工业 
环境 的 严格 标准 ， 不 过 随 着 ROS-Industrial 的 不 断 发 展 和 完善 ， 将 ROS 应 用 到 工业 领域 将 变 成 现实 。 


9.3.2 工业 机 械 辟 Descartes 运 动 规划 


1. 功 能 简介 
Descartes 是 ROS-Industrial 项 目的 内 容 ， 其 设计 初衷 是 实现 约束 条 件 下 的 Cartesian 轨 迹 路 径 规划 任务 。 


该 接口 使 用 Cartesian 路 径 规 划 的 Movelt， 主 要 用 于 规划 具有 Cartesian 路 径 的 机 器 人 动作 轨迹 。 该 接口 所 包含 的 单独 结构 能 够 把 Cartesian 规 划 路 径 转 化 成 关节 规划 路 径 。 由 于 该 接口 不 同 于 标准 逆 运 
动 学 方法 (Cartesian 路 径 需 要 完全 定义 ) ， 因 此 其 比较 适用 于 工业 应 用 。 


当前 MoveltVROS 接 口 主要 用 于 抓 取 和 放置 等 应 用 ， 而 在 典型 的 抓 取 和 放置 应 用 中 ， 机 器 人 初始 位 置 和 目标 位 置 通常 被 作为 规划 器 的 输入 。 但 在 喷涂 、 加 工 和 焊接 等 工业 应 用 场景 中 ， 机 器 人 轨迹 必须 
遵循 Cartesian 路 径 ， 其 不 仅 与 开始 位 置 和 结束 位 置 相关 ， 其 运动 过 程 也 同样 重要 。 解 决 Cartesian 路 径 规 划 问 题 如 果 仪 考虑 采用 具有 约束 的 逆 运动 学 解决 方案 则 会 缺乏 灵活 性 ， 在 实际 应 用 中 ，Cartesian 路 
径 


具有 典型 的 半 约 束 特点 ， 如 在 工业 加 工 应 用 中 ， 仅 需要 5 个 自由 度 的 路 径 规 划 ， 而 对 于 第 六 自由 度 的 方向 并 没有 定义 。 传 统 路 径 规 划 器 并 没有 利用 这 些 无 约束 关节 的 优势 ， 从 而 限制 了 找到 最 优 方案 的 可 能 
性 。 
2. 工 作 原理 


从 软件 体系 架构 的 视角 来 看 ，Descartes 可 使 用 轨迹 点 、 机 器 人 模型 及 规划 器 生成 满足 给 定 约束 的 关节 轨迹 ， 如 图 9-5 所 示 。 
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图 9-5 ”Descartes 体系 架构 


1) 轨迹 点 : 路 径 时 序 定义 的 点 序列 ， 这 些 点 刻画 了 基于 用 户 需要 搜索 有 效 关节 轨迹 方案 的 逻辑 顺序 。 

2) 机 器 人 模型 : 定义 机 器 人 完成 给 定 轨迹 的 物理 特性 ， 完 成 对 正 逆 向 运动 学 解 算 和 轨迹 有 效 性 的 检查 。 

3) 规划 器 : Descartes 体 系 结构 的 最 高 层 的 组 件 ， 主 要 通过 轨迹 点 队列 为 给 定 的 机 器 人 模型 生成 有 效 且 可 优化 的 解决 方案 。 

3. 核 心 实 现 

1) Descartes_core: 定义 机 器 人 模型 、 轨 迹 点 和 路 径 规 划 器 这 些 核心 概念 的 抽象 接口 ， 包 括 TrajectoryPt、RobotModel 和 PathPlannerBase 三 部 分 。 
2) Descartes moveit: 该 部 分 通过 Movelt 实 现 Descartes 机 器 人 模型 ， 同 时 提供 正 逆 向 运动 学 解 算 和 配置 的 有 效 性 检查 。 


3) Descartes_planner: Descartes 规 划 器 用 于 对 给 定 轨 人 迹 搜 索 有 效 的 机 器 人 配置 ， 同 时 对 关节 配置 序列 生成 解决 方案 。Descartes 提 供 了 两 种 轨迹 规 划 器 的 参考 实现 ， 通 过 接收 一 个 任意 轨迹 点 序列 作 
为 输入 ， 来 输出 关节 轨迹 点 序列 。 


` 稠密 规划 器 

规划 器 接收 稠密 的 轨迹 点 ， 通 过 轨迹 点 的 抽样 逻辑 及 机 器 人 模型 的 静 动 态 特点 ， 找 到 能 够 到 达 所 有 点 的 路 径 ， 并 且 使 得 关节 运动 最 小 化 。 其 操作 步骤 具体 如 下 。 

a) 对 于 每 个 轨迹 点 ， 计 算 所 有 关于 该 点 的 逆 运 动 学 解 算 。 

b) 对 于 每 一 对 连续 的 点 ， 计 算 移动 代价 及 解决 方案 的 可 行 性 。 

c) 利用 已 知 数据 创建 图 ， 图 中 的 顶点 代表 运动 学 解决 方案 ， 边 代表 移动 代价 。 

d) 利用 Dijkstra 算 法 搜索 从 任意 初始 配置 到 结束 配置 的 最 小 代价 路 线 。 

黎 朴 规划 器 

稀 玖 规划 器 建立 在 稠密 规划 器 原理 的 基础 之 上 ， 通 过 采样 给 定 的 轨迹 点 来 节省 计算 时 间 。 稀 跑 规划 器 在 初始 点 的 子 集 上 进行 逆 运 动 学 求解 ， 同 时 通过 插值 和 正 向 运动 学 处 理 余下 的 轨迹 点 。 
例如 ， 如 果 规 划 器 需要 处 理 A、B、( 人 三 个 点 ， 那 么 稀 中 规 划 器 就 会 使 用 A 和 (C 来 进行 逆 运 动 学 求解 ， 然 后 把 B 插 入 到 解决 方案 中 。 稀 琉 规划 器 会 通过 正 运动 学 检查 该 方案 ， 并 和 目标 位 置 进行 比较 。 
+ 局 限 性 和 失效 模式 

如 果 关 节 中 的 点 机 器 人 模型 无 法 到 达 ， 或 天 节点 能 够 到 达 ， 但 需要 的 配置 超过 了 一 定 的 阔 值 ， 那 么 这 些 情况 下 规划 器 是 不 适用 的 。 

4) Descartes trajectory: 该 部 分 给 出 了 一 般 的 轨迹 点 参考 实现 ， 可 以 指定 确定 的 Descartes 姿 态 或 确定 的 关节 配置 。 

4. 与 Movelt 的 区 别 

Descartes 作 为 路 径 规划 库 ， 与 ROS 中 路 径 规划 库 Movelt 的 不 同 之 处 主要 体现 在 以 下 几 个 方面 。 


1) Cartesian 规 划 : Movelt 规 划 是 无 约束 的 空间 运动 ，Descartes 规 划 路 径 是 半 约 束 的 Cartesian 路 径 。 


2) 高 效 、 可 重复 和 尺度 可 调 : Descartes 生 成 的 路 径 接 近 人 类 的 规划 路 径 ， 根 据 问 题 的 复杂 性 ， 路 径 规 划 是 可 重复 的 并 且 尺 度 是 可 调 的 。 简 单 路 径 能 够 快速 规划 ， 复 杂 路 径 则 比较 耗 时 。 
3) 动态 重 规划 : 路 径 规划 一 旦 完成 ， 规 划 结 构 就 被 存放 在 内 存 中 。 当 路 径 发 生 改变 时 ， 路 径 规划 能 够 立即 生成 新 的 机 器 人 关节 轨迹 。 


4) 离线 规划 : Descartes 的 初衷 是 用 于 离线 规划 和 动作 等 应 用 ， 实 时 性 不 是 Descartes 的 特征 。 


9.3.3 ”特定 工业 机 器 人 MeotoPlus-ROS 增 量 运动 实现 


1. 架 构 设计 


Motoplus-ROS 架 构 主要 实现 了 ROS 节 点 与 Motoman 运 动 控 制 器 之 间 的 通信 与 协同 ， 在 该 架构 中 ，ROS 作 为 顶层 的 控制 节点 实现 对 Motoman 工 业 机 器 人 的 运动 规划 和 运动 控制 等 功能 。ROS 层 主要 生 
成 高 层 的 路 径 规划 结果 ，ROS-Industrial 层 对 路 径 进 行 分 解 ， 增 加 控制 信息 ，FS100 控 制 器 通过 与 ROS-Industrial 层 交互 ， 获 取 路 径 信息 ， 通 过 插值 算法 ， 生 成 机 器 人 运动 控制 指令 ， 如 图 9-6 所 示 。 
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图 9-6 ”MotoPlus-ROS 体 系 结 构 


2.MotoROS 和 和 ROS 通信 
ROS-Industrial 主 要 任务 是 生成 路 径 点 ， 然 后 发 送 给 机 器 人 控制 器 。ROS 内 部 生成 路 径 点 和 速度 信息 ， 通 过 TCP/IP 协 议 发 送 给 MotoPlus 应 用 ， 运 行 在 控制 器 上 的 MotoPlus 解 析 并 使 用 这 些 信息 。 一 旦 
MotoROS 接 收 到 路 径 点 ， 就 会 立即 反馈 确认 信息 ， 然 后 处 于 接收 后 续 路 径 的 就 绪 状态 。 


MotopPlus 运 行 有 序 地 告知 任务 来 移动 机 器 人 ， 而 不 需要 任何 的 运动 指令 。 只 有 当 处 于 等 待 指令 状态 时 ，mpExRcslncrementMove 指 令 才 会 执行 。 告 知 任务 的 具体 形式 如 下 : 








NOP // 重 置 I/o 信 和 号 


DOUT OT# (890) OFF 

DOUT OT# (889) OFF 

TIMER T=0.05 // 向 ROS 发 送信 号 ， 告知 控制 器 处 于 接收 动作 的 ready 状 态 
DOUT OT# (889) ON // 等 待 ROS 完 成 信号 

WAIT OT# (890) ON // 关闭 控制 器 ready 信 号 
































DOUT OT# (890) OFF 
END 








3. 通 信 队 列 与 任务 
(1) 主任 务 (Main Task) 


主任 务 是 启动 后 的 初始 化 任务 ， 首 先 创 建 一 个 包含 MotoPlus 应 用 所 有 数据 的 控制 器 结构 实例 ， 启 动 连接 服务 器 任务 ， 之 后 进入 循环 等 待 监 控 控 制 器 的 状态 ， 一 旦 控制 器 的 状态 发 生 改 变 ， 主 任务 就 执行 
相应 的 处 理 ， 如 图 9-7 所 示 。 
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图 9-7 主任 务 示 意图 
(2) 连接 服务 器 任务 (Connection Server Task) 


如 图 9-8 所 示 ， 连 接 服务 器 任务 开放 多 个 端口 (状态 、 监 控 等 ) ， 等 待 客户 端 连接 其 中 的 某 个 端口 。 一 旦 某 个 端口 被 检测 到 建立 连接 ， 相 应 的 新 任务 就 启动 处 理 与 连接 的 客户 端 间 的 通信 。 该 体系 结构 支 
持 多 个 客户 端 连 接 同 一 个 服务 器 ， 但 该 模式 有 连接 上 限 ， 超 过 连接 上 限 的 连接 请 求 会 被 拒绝 。 最 初 设计 支持 端口 和 相应 服务 器 的 任务 只 是 状态 服务 器 和 动作 服务 器 ， 以 后 会 添加 更 多 类 型 的 服务 。 
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图 9-8 ”连接 服务 器 任务 示意 图 


(3) 状态 服务 器 任务 (State Server Task) 


如 图 9-9 所 示 ， 状 态 服务 器 检索 控制 器 状态 。 然 后 把 状态 信息 发 送 给 连接 到 相应 状态 服务 器 端口 的 客户 端 。 在 这 一 过 程 中 ， 客 户 端 完 全 不 需要 发 送 任 何 请 求 ， 一 旦 连接 建立 ， 状 态 信 息 就 会 周期 性 地 发 送 
到 客户 端 。 因 此 可 能 存在 多 个 客户 端 在 监听 同一 消息 ， 但 是 只 有 一 个 状态 服务 器 任务 的 实例 的 情况 。 


(4) 动作 服务 器 任务 (Motion Server Task) 


如 图 9-10 所 示 ， 在 与 动作 服务 器 端口 建立 连接 时 ， 动 作 服务 器 任务 会 立即 启动 。 但 是 多 个 连接 并 不 能 控制 相同 的 控制 组 。 因 此 ， 在 最 初 设计 时 ， 只 是 关注 单一 的 机 器 人 ， 即 仪 支持 单一 的 连接 。 当 多 个 
机 器 人 需要 控制 时 ， 如 果 发 送 的 动作 信息 包含 所 有 控制 组 的 动作 ， 那 么 只 有 其 中 的 某 个 连接 是 有 效 的 。 但 是 ， 如 果 控 制 组 仅 被 它 拥有 的 节点 (该 节点 只 向 该 控制 组 发 送 控制 信息 ) 所 控制 ， 那 么 每 个 组 的 连 
接 都 是 有 效 的 。 
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图 9-10 动作 服务 器 任务 示意 图 
动作 服务 器 任务 需要 等 待 客服 端的 动作 消息 ， 当 收 到 消息 时 ， 动 作 服 务 器 会 识别 消息 的 类 型 和 目的 ， 然 后 采取 相应 动作 ， 最 后 通过 MOTION_REPLY 消 息 返 回 给 客户 端 相应 的 处 理 结 


如 果 轨 人 迹 点 的 队列 处 于 满 状 态 时 ， 用 来 生成 增 量 动作 的 轨迹 点 动作 消息 的 回复 会 被 延迟 。 这 种 情况 是 不 允许 出 现 的 ， 因 为 停止 动作 也 会 被 拒绝 进入 队列 。 因 此 为 了 防止 这 种 状况 的 发 生 ， 消 息 首先 会 被 
识别 和 解析 ， 轨 人 迹 点 数据 会 被 临时 复制 到 AddTolnc-QueueProcess 后 台 任 务 进行 处 理 。 因 此 在 这 种 结构 下 ， 成 功 的 应 答 消 息 仅仅 意味 着 消息 被 正确 地 接收 ， 并 不 意味 着 这 个 动作 已 经 被 转化 为 增 量 运 动 。 
如 果 后 台 任 务 正在 处 理 一 个 轨迹 点 ， 那 么 会 为 客户 端 返回 服务 器 忙 的 回复 ， 客 户 端 需要 重新 发 送 这 个 消息 。 


(5) 添加 增 量 运动 队列 任务 (Add To Inc Move Queue Task) 


AddTolncQueueProcess 属 于 后 台 任 务 ， 主 要 负责 把 动作 消息 的 轨迹 数据 分 解 成 动作 的 片段 ， 以 此 来 匹配 控制 器 的 插值 周期 。 如 果 存 在 多 个 动作 服务 器 连接 的 情况 ， 那 么 每 一 个 动作 服务 器 都 需要 创建 
一 个 AddlncMoveQueue 任 务 ， 如 图 9-11 所 示 。 
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图 9-11 添加 增 量 运动 队列 任务 示意 图 
(6) 增 量 运 动 任 务 (Inc Move Queue) 


如 图 9-12 所 示 ， 增 量 运 动 任务 和 控制 器 内 部 时 钟 同步 ， 同 时 根据 运动 插值 周期 通过 mpExRcsincrementMove 把 增 量 运 动 传递 给 控制 器 。 这 样 操 作 是 为 了 使 得 运动 的 速度 正 合 适 。 如 果 不 考虑 动作 服务 
器 的 数量 ， 则 只 能 存在 一 个 增 量 运 动 任务 的 实例 。 


4. 数 据 结构 
(1) 控制 器 结构 


控制 器 结构 是 主要 的 数据 结构 ， 它 包含 了 MotoROS 应 用 的 所 有 数据 。 在 启动 初始 化 时 ， 通 过 控制 器 数据 进行 初始 化 。 控 制 器 数据 结构 拥有 控制 器 的 详细 信息 ， 例 如 插值 周期 、 控 制 器 组 数量 及 不 同 任务 


指针 等 。 指 向 控制 器 结构 的 指针 通常 作为 函数 参数 传递 。 在 初始 化 阶段 ， 控 制 器 数据 通过 调用 ParameterExtraction 库 中 的 函数 进行 添加 操作 。 
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图 9-12” 增 量 运 动 任务 示意 图 
(2) 控制 组 


控制 组 结构 (ctrlGroup) 是 控制 器 主要 的 子 结构 之 一 ， 它 包含 控制 组 的 详细 信息 。 一 个 控制 组 就 是 一 个 具备 多 个 坐标 轴 的 逻辑 组 ， 该 逻辑 组 可 以 是 机 器 人 、 底 座 或 站 。 例 如 ， 单 臂 机 器 人 只 有 一 个 组 
， 但 对 于 双 臂 机器人， 就 具备 组 R1、R2、B1、B2。 每 个 控制 组 将 拥有 相对 于 它 的 坐标 系 的 详细 数据 ， 主 要 包括 坐标 系数 量 、 脉 冲 频 率 及 速度 限制 等 。 每 个 控制 组 都 拥有 自己 的 增 量 运 动 队 列 ， 因 此 每 个 
控制 组 都 可 以 单独 运动 而 不 受 其 他 组 的 影响 。 


(3) 增 量 运动 队列 


增 量 运 动 队列 是 固定 长 度 的 循环 队列 ， 并 且 是 可 以 复 用 的 。 队 列 支 持 获得 由 若干 点 组 成 的 轨迹 ， 以 较 低 的 频率 发 送 给 MotoROS， 并 且 依 据 插值 周期 把 轨迹 拆 分 成 足够 小 的 增 量 。 这 些 由 增 量 队 列 保存 的 
足够 小 的 增 量 可 以 通过 mpExRcsincrementMoce 消 数 以 合适 的 插值 频率 发 送 给 控制 器 。 


(4) 简单 消息 


ROS-Indutrial 通 过 简单 消息 结构 实现 ROS 与 运行 在 控制 器 上 MotoROS 的 通信 。 简 单 消息 是 由 前 级、 消息 头 和 数据 组 成 的 。 前 缀 包含 消息 的 长 度 (header+data) ， 消 息 头 包含 消息 的 格式 、 通 信和 的 类 
型 和 应 答 ; 数据 形 式 多 种 多 样 ， 主 要 取决 于 消息 的 类 型 ，ROS-Industrial 已 经 定义 了 一 些 基本 类 型 ， 但 是 这 些 消息 并 不 能 很 好 地 满足 Motoman 控 制 器 的 数据 要 求 。 为 此 添加 了 专用 的 Motoman 消 息 ， 同 时 
也 尽 可 能 地 遵循 标准 的 消息 格式 。 


ROBOT STATUS (13) : 机 器 人 状态 消息 ， 主 要 表示 机 器 人 或 控制 器 的 状态 。 

JOINT TRAJ_PT_FULL (14) : 关节 轨迹 点 消息 ， 包 括 位 置 、 速 度 、 加 速度 和 时 间 。 该 类 型 消息 提供 了 足够 的 轨迹 数据 ， 精 确 表示 由 ROS 速 度 匹 配 之 后 生成 的 精细 轨迹 。 
JOINT_FEEDBACK (15) : 关节 反馈 消息 ， 包 括 位 置 、 速 度 、 加 速度 和 时 间 。 

MOTO_MOTION_CTRL (2001) : Moto 动 作 控 制 消 息 ， 主 要 用 来 发 送 动作 命令 实现 运动 控制 ， 以 及 管理 整个 运动 。 

MOTO MOTION_REPLY (2002) : 当 MotoROS 收 到 关节 轨迹 消息 或 Motoman 动 作 控 制 命令 时 ，MotoROS 会 发 送 这 个 应 答 消息 。 


5. 动 作 就 绪 状态 


为 了 使 控制 器 能 够 收 到 外 部 的 增 量 运动 参数 ， 控 制 器 必须 处 于 MOTION_READY 状 态 。 该 状态 表示 控制 器 准备 接收 ROS 的 轨迹 信息 。 就 绪 状态 需要 满足 以 下 条 件 。 
- Operating: 作业 处 于 回放 模式 。 
- WAIT instruction: 作业 正在 执行 WAIT 指 令 。 但 是 并 没有 明确 的 方式 检测 WAIT 指 令 ， 不 过 可 以 发 送 mpExRcsIncrementMove， 如 果 没 有 WAIT 指 令 正 在 执行 ， 返 回 值 为 错误 。 
- HandshakingI/O 信 号 处 于 开启 状态 : 由 于 WAIT 指 令 不 容易 检测 ， 所 以 I/O 信 号 设置 在 WAIT 指 令 之 前 以 此 来 标识 WAIT 指 令 已 经 到 达 。 


: REMOTE: 系统 处 于 REMOTE 状 态 ，mpExRcsIncrementMove 其 实 并 不 需要 控制 器 处 于 REMOTE 状 态 ， 但 是 其 他 用 来 管理 系统 的 MotoPlus 函 数 出 于 安全 的 考虑 需要 这 样 的 要 求 ， 因 此 这 个 条 件 也 被 添 


加 进来 。 


MOTION_READY 状 态 可 以 通过 发 送 MOTO_MOTION_CTRL 消 息 来 检测 。 大 多 数 系统 扰动 会 关闭 MOTION_READY 信 和 号， 扰动 包括 警告 、 错 误 、E-stop、REMOTE、PLAYH 和 TEACH 状 态 的 切换 及 伺 
服 关 闭 等 。 


当 MOTION_READY 状 态 关 闭 时 ， 所 有 后 续 的 关节 轨迹 点 消息 将 会 被 拒绝 ， 同 时 返回 NOT_READY 应 答 消息 ， 增 量 队列 会 被 清空 ， 后 续 增 量 不 会 发 送 给 控制 器 。 重 启 时 ，MOTION_READY 状 态 需要 重 
建 ，ROS 会 利用 当前 机 器 人 的 启动 位 置 重新 生成 新 的 轨迹 。 


系统 未 处 于 MOTION_READY 状 态 时 ， 发 送 MOTO_MOTION_CTRL 消 息 会 启动 下 面 的 序列 使 得 控制 器 处 于 合适 的 状态 : 重 置 警报 和 错误 、 伺 服 开启 、 选 择 作业 和 启动 作业 执行 。 
6.MotoROS 与 控制 器 通信 


MotoROS 通 过 MotoPlus API 实 现 与 控制 器 间 的 数据 交换 。MeotoPlus 是 专业 的 集成 开发 环境 ， 利 用 C 语 言 在 MotoPlusIDE 文 本 编辑 器 中 编写 代码 ， 然 后 编译 链接 成 可 执行 程序 ， 下 载 到 机 器 人 控制 器 
中 ， 可 作为 任务 在 机 器 人 内 执行 。 机 器 人 上 电 后 ， 存 储 在 CF 卡 上 的 MotoPlusApl 应 用 程序 ， 自 动 导 入 到 机 器 人 内 存 中 ， 并 与 系统 程序 同时 执行 。 


MotoPlus 应 用 程序 是 以 任务 的 形式 启动 的 ， 应 用 程序 可 以 是 单 任务 ， 也 可 以 是 多 任务 ， 可 以 同时 启动 ， 也 可 以 在 任务 内 单独 调用 。 任 务 具 有 和 运行、 睡眠、 等 待 等 多 种 状态 。 根 据 任 务 功能 和 重要 程度 的 
不 同 ， 还 可 以 设置 不 同 的 优先 级 ， 不 同 任务 间 可 通过 信号 或 事件 进行 信息 交互 。 


MotoPlus 的 API 提 供 的 功能 包括 任务 控制 、 机 器 人 控制 、JOB 控 制 、1O 控 制 、 网 络 通信 控制 、 事 件 和 串口 通信 控制 等 。 利 用 这 些 功 能 ， 可 以 通过 视觉 传感器 或 其 他 传感器 的 数据 动态 调整 机 器 人 的 位 
置 ， 通 过 网 络 通信 实现 机 器 人 与 外 部 PC 的 数据 传输 ， 或 者 机 器 人 程序 的 顺序 控制 。 


7. 脉 冲 增 量 插 补 算法 


MotoROS 可 以 从 ROS-Industrial 接 收 轨迹 点 数据 ， 其 包含 序列 编号 、 时 间 故 、 绝 对 位 置 和 角 弧 度 表示 的 速度 。 处 于 某 一 路 径 上 的 点 具备 不 同 空 间 和 时 间 间 隙 信息 ，Moto-ROS 应 用 需要 对 这 些 点 表示 的 
路 径 进行 插 补 ， 从 而 依据 控制 器 规定 的 插 补 周期 确定 发 送 给 控制 器 的 相应 增 量 运动 。 加 速度 是 线性 方程 ， 取 决 于 位 置 、 速 度 和 时 间 等 数据 。 


该 算法 的 主要 思想 是 在 两 个 ROS 控 制 点 中 增加 插 补 点 。 已 知 的 数据 是 从 ROS 接 收 到 的 轨迹 消息 ， 包 括 位 置 、 速 度 和 时 间 (这 些 都 是 ) 。 该 算法 还 需要 检查 增 量 运动 的 有 效 性 ， 以 确保 任何 通过 ROS 发 送 
的 极端 值 都 不 会 对 机 械 手 造成 损坏 。 


94 本 童 小 结 


本 章 主要 着 眼 于 ROS 在 工业 智能 机 器 人 上 的 应 用 ， 并 介绍 了 ROS-Industrial， 作 为 ROS 面 向 工业 领域 的 拓展 ，ROS-Industrial 在 实验 平台 上 不 断 地 得 到 验证 。 同 时 本 章 还 针对 ROS-Industrial 的 具体 内 容 
进行 了 详尽 的 描述 ， 探 完了 其 设计 思想 及 其 实现 原理 。 通 过 ROS-Industrial 在 工业 机 器 人 上 的 具体 实现 实例 ， 进 一 步 了 解 了 ROS-Industrial 的 具体 应 用 场景 及 实现 过 程 中 的 具体 细节 ， 同 时 通过 在 motoman 
工业 机 器 人 上 的 具体 实现 ， 还 能 够 看 出 如 何 应 用 ROS-Industrial 中 的 工业 机 器 人 客户 端 及 简单 消息 协议 等 内 容 。 


但 同时 也 能 够 发现 ，ROS-Industrial 的 具体 实现 仍 处 于 实验 测试 阶段 ， 离 具体 的 实际 应 用 还 有 距离 。 随 着 ROS-Industrial 项 目的 不 断 深入 ，ROS-Industrial 的 内 涵 也 会 更 加 丰富 。 就 像 智 能 手机 行业 的 
Android 一 样 ，ROS 和 ROS-Industria| 将 会 成 为 机 器 人 领域 非常 强大 的 使 能 技术 ， 同 时 ROS-Industrial 在 开源 机 器 人 软件 平台 的 行业 标准 也 在 逐渐 完备 。 因 此 ，ROS 和 ROS-Industrial 是 机 器 人 开发 领域 的 先 
行者 ， 随 着 越 来 越 多 的 行业 对 其 认识 的 不 断 加 深 , 会 有 更 多 的 机 器 人 企业 和 研发 人 员 参 与 其 中 ， 共 同 推动 ROS 的 快速 发 展 。 


第 10 章 ”智能 机 器 人 软件 平台 及 其 未 来 友 展 


10.1 其 他 智能 机 器 人 软件 平台 
除 ROS 之 外 ， 一 些 世界 著名 的 专业 化 机 器 人 机 构 及 IT 公司 也 研发 了 功能 类 似 、 各 具 特 色 的 智能 机 器 人 软件 平台 。 例 如 ， 瑞 典 ABB 持 续 研发 RobotWare 系 列 ， 其 拥有 强大 的 扩展 开发 工具 及 通信 能 力 ， 支 
持 机 器 人 软件 编程 与 控制 优化 ， 从 而 具有 很 高 的 灵活 性 与 可 靠 性 ;德国 Kuka 的 智能 机 器 人 软件 平台 支持 多 种 轨迹 控制 ， 研 制 的 7 轴 轻 型 人 机 协作 机 器 人 LBR iiwa 具 有 人 机 协同 操作 及 高 性 能 碰撞 检测 能 


日 本 Motoman 提 供 了 丰富 的 二 次 开发 工具 包 和 多 类 API 消 数 ;美国 微软 机 器 人 工作 室 推出 的 机 器 人 操作 系统 MSRS 具 有 可 视 化 编程 语言 、 机 器 人 服务 和 机 器 人 仿真 三 大 功能 。 本 章 将 会 简要 介绍 这 几 种 典型 
系统 。 
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10.1 其 他 智能 机 器 人 软件 平台 


除 ROS 之 外 ， 一 些 世 界 著名 的 专业 化 机 器 人 机 构 及 IT 公 司 也 研发 了 功能 类 似 、 各 有 具 特色 的 智能 机 器 人 软件 平台 。 例 如 ， 瑞 典 ABB 持 续 研发 RobotWare 系 列 ， 其 拥有 强大 的 扩展 开发 工具 及 通信 能 力 ， 支 
持 机 器 人 软件 编程 与 控制 优化 ， 从 而 具有 很 高 的 灵活 性 与 可 靠 性 ; 德国 Kuka 的 智能 机 器 人 软件 平台 支持 多 种 轨迹 控制 ， 研 制 的 7 轴 轻 型 人 机 协作 机 器 人 LBR iiwa 具 有 人 机 协同 操作 及 高 性 能 碰撞 检测 能 


日 本 Yotoman 提 供 了 丰富 的 二 次 开发 工具 包 和 多 类 API 函 数 ; 美国 微软 机 器 人 工作 室 推出 的 机 器 人 操作 系统 MSRS 具 有 可 视 化 编程 语言 、 机 器 人 服务 和 机 器 人 仿真 三 大 功能 。 本 章 将 会 简要 介绍 这 几 种 典型 
系统 。 


10.1.1 ”微软 机 器 人 软件 平台 


微软 机 器 人 软件 平台 (Microsoft RoboticsStudio, MSRS) 是 微软 公司 在 2006 年 公开 发 行 的 ， 目 的 是 试图 为 机 器 人 控制 提供 一 个 工业 标准 。MSRs 的 体系 设计 遵从 有 代表 性 的 状态 转移 模式 ， 通 过 使 
用 多 种 软件 服务 实现 机 器 人 的 互 操作 ， 类 似 于 Web Service。 所 定义 的 服务 之 间 高 度 独立 ， 没 有 相关 关系 ， 因 此 ， 它 为 良好 的 代码 重用 提供 了 可 能 性 。MSRs 采 用 分 布 式 消息 机 制 实现 不 同 服务 之 间 的 通信 ， 
使 得 服务 之 间 既 可 以 在 同一 台 机 器 人 或 节点 之 间 进 行 通信 ， 也 可 以 通过 不 同类 型 的 网 络 协议 实现 远程 通信 。 服 务 模型 架构 和 机 器 人 定义 的 广义 特性 使 得 开发 者 可 以 在 操作 和 设计 机 器 人 等 方面 广泛 使 用 
MSRS。MSRS 所 定义 的 服务 不 仅仅 只 有 控制 驱动 和 传 感 交 互 ， 同 时 也 包括 基于 Web 的 错误 报告 、 无 线 通 信 ， 等 等 。 


\ 一 /一 


机 器 人 设计 套件 (Robotics Design Studio, RDS) 是 MSRs 中 的 开发 环境 。RDSs 包 括 一 个 运行 时 环境 、 一 个 仿真 器 、 一 个 可 视 化 编程 语言 及 一 套 工 具 。 运 行 时 环境 可 以 工作 在 目前 的 机 器 人 主要 使 用 的 
各 种 8 位 、16 位 和 32 位 处 理 器 上 。 其 作用 是 可 让 用 户 编写 简单 的 模块 化 命令 程序 ， 并 如 同 服务 那样 活动 。 这 种 程序 一 般 不 在 目标 机 器 人 的 有 限 处 理 器 和 人 存储 器 上 运行 ， 而 是 通过 机 器 人 定义 的 某 种 通信 协议 
与 机 器 人 进行 交互 。 例 如 ，iRobot 公 司 的 Roomba 真 空 吸尘器 是 最 受 欢 迎 的 消费 类 机 器 人 之 一 ， 至 今 销量 已 经 超过 100 万 台 ， 其 采用 的 是 《 思 卡 尔 半 导体 公司 提供 的 16 位 简单 处 理 器 。 该 处 理 器 不 具有 用 户 
可 访问 的 存储 器 ， 而 是 通过 品行 接口 协议 与 PC 的 蓝牙 链接 ， 以 便 给 机 器 人 发 命令 。RDS 还 提供 可 创建 真实 三 维 效 果 的 仿真 环境 ， 这 得 益 于 Aegia 技 术 公 司 提供 的 第 三 方 物理 引擎 。 开 发 人 员 可 以 充分 测试 程 
序 ， 而 不 用 冒 机 器 人 可 能 从 楼 梯 上 摔 下 来 的 风险 。 


在 RDS 提 供 的 众多 工具 中 ， 可 视 化 编程 语言 作为 主要 工具 可 以 帮助 编程 人 员 通 过 拖 放 图 标 快速 创建 命令 程序 。 例 如 ， 工 具 可 以 让 用 户 将 机 器 人 图 标 与 视频 游戏 机 的 操纵 杆 相 关联 ， 从 而 可 以 用 操纵 杆 控 
制 机 器 人 的 移动 。 微 软 的 RDs 环 境内 包含 30 多 个 编程 指南 ， 其 中 大 多 数 已 提供 源 代 码 ， 可 实现 基本 文本 、 语 音 或 照相 机 等 功能 。RDS 环 境 可 运行 在 Windows XP 和 Vista 上 ， 并 可 将 它 移植 到 Windows CE 
上 ， 一 些 机 器 人 程序 使 用 Windows CE 作为 固有 操作 系统 。 


RDS 软 件 基本 架构 如 图 10-1 所 示 。 
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图 10-1 RDS 结 构 
RDSs 提 供 各 种 高 层 服务 ， 其 一 般 依赖 于 低层 服务 提供 的 抽象 ， 实 现 设 备 之 间 的 通信 。 但 有 些 高 层 服 务 是 基于 更 底层 的 功能 独立 运行 的 。 


底层 服务 可 由 机 器 人 的 制造 者 提供 ， 或 者 由 开发 者 建立 自己 的 机 器 人 功能 描述 。 这 些 服务 使 用 机 器 人 的 1/O 控 制 器 进行 通信 。 它 们 类 似 于 一 个 硬件 抽象 层 ， 并 且 定 义 了 高 层 服务 的 调用 接口 。RDS 中 还 包 
含 了 基于 Kinect 和 其 他 设备 的 服务 。 


并 发 协同 运行 时 (CCR) 和 分 布 软件 服务 (DSS) 是 RDS 的 两 个 核心 组 件 。 应 用 程序 通过 有 效 使 用 CCR 可 以 获得 更 好 的 响应 能 力 ， 以 及 更 好 的 伸缩 性 及 容错 性 。 而 所 有 基于 DSS 节 点 上 下 文 运行 的 服务 
和 多 个 DSS 节 点 能 够 通过 网 络 进行 通信 。 


RDS 是 基于 .NET 架 构 的 ， 且 运行 在 Windows 平 台 之 上 ， 这 是 一 个 先决 条 件 。 


最 底层 的 是 硬件 层 ， 诸 如 无 线 XBOX 控 制 器 等 硬件 ， 都 拥有 相应 的 Windows 驱 动 。 机 器 人 1/O 控 制 器 板 卡 上 已 有 串口 、USB、 网 络 和 其 他 方法 进行 通信 的 固件 。 


10.1.2 ”ABB 智 能 机 器 人 软件 平台 


ABB 作 为 国际 著名 的 工业 智能 机 器 人 公司 ， 自 主 研发 了 功能 丰富 的 机 器 人 软件 平台 ， 主 要 由 RobotStudio 和 RobotWare 两 部 分 组 成 ， 分 别 用 于 工业 智能 机 器 人 的 开发 和 控制 |。 


1. 智 能 机 器 人 软件 开发 工具 RobotStudio 


智能 机 器 人 软件 开发 工具 RobotStudio 是 离线 可 视 化 编程 工具 和 仿真 工具 。 该 工具 以 ABB 独 特 的 虚拟 机 器 人 技术 为 基础 ， 运 行 环境 为 实际 使 用 的 真实 机 器 人 配置 文件 ， 与 机 器 人 在 实际 生产 中 运行 的 软 
件 完全 一 致 ， 因此， 通过 RobotStudio 可 执行 十 分 台 真 的 模拟 。 基 于 虚拟 机 器 人 技术 进行 离线 编程 ， 不 仅 如 同 将 真实 的 机 器 人 搬 到 PC 宿主 机 中 ， 可 直观 快速 地 验证 机 器 人 应 用 软件 ， 还 可 以 尝试 不 同 的 搭配 
以 选 出 最 佳 的 解决 方案 ， 而 且 借 助 ABB 模 拟 与 离线 编程 软件 ， 可 在 办 公 室 内 完成 机 器 人 软件 的 编程 和 优化 ， 无 须 中 断 机 器 人 生产 过 程 ， 并 且 可 在 机 器 人 硬件 制造 的 同时 ， 测 试 新 机 器 人 软件 及 其 系统 ， 以 提 
高 整体 生产 效率 


RobotStudio 的 主要 工具 及 其 功能 简 述 如 下 。 


. CAD 导 入 : RobotStudio 可 方便 地 导入 各 种 主流 CAD 格 式 数据 ， 包 括 IGES、STEP、VRML、VDAFS、ACIS 及 CATIA 等 。 机 器 人 程序 设计 者 可 依据 这 些 精 确 数据 编程 实现 精度 更 高 的 机 器 人 程序 ， 从 而 


- 自动 路 径 : 通过 使 用 待 加 工 零 件 的 CAD 模 型 ， 在 数 分 钟 之 内 可 自动 生成 跟踪 加 工 曲线 所 需要 的 机 器 人 路 径 ， 而 这 项 任务 以 往 通常 需要 数 小 时 甚至 数 天 ， 因 此 ， 该 功能 能 够 有 效 节省 工业 智能 机 器 人 的 
应 用 开发 时 间 。 


. 程序 编辑 器 : 可 生成 机 器 人 程序 ， 使 用 户 能 够 在 Windows 环 境 中 离线 开发 或 维护 机 器 人 程序 ， 可 显著 缩短 编程 过 程 、 改 进程 序 结构 。 


- 路 径 优 化 : 如 果 程 序 包 含 接近 奇异 点 的 机 器 人 动作 ，RobotStudio 可 自动 检测 出 来 并 发 出 警报 ， 从 而 防止 机 器 人 在 实际 运行 中 发 生 此 现象 。 通 过 仿真 监视 器 优化 速度 、 加 速度 、 奇 异 点 或 轴线 等 参数 ， 
缩短 周期 时 间 ， 使 机 器 人 按照 最 有 效 的 方式 来 运行 


达 分 析 : 用 户 可 通过 该 功能 任意 移动 机 器 人 或 工件 ， 直 到 所 有 位 置 均 可 到 达 ， 在 数 分 钟 之 内 可 完成 工作 单元 平面 布置 验证 和 优化 。 
ER: 一 种 用 于 验证 程序 结构 与 远 辑 的 调试 工具 。 程 序 在 执行 期 间 ， 可 通过 该 工具 直接 观察 工作 单元 的 1/O 〇 状态。 可 将 I/O 〇 连接 到 仿真 事件 ， 实 现 工 位 内 机 器 人 及 所 有 设备 的 仿真 。 
+ 碰撞 检测 : 选 定 检测 对 象 后 ， 碰 撞 检 测 功 能 可 自动 监测 并 显示 程序 执行 时 这 些 对 象 是 否 会 发 生 碰撞 ， 可 避免 设备 碰撞 造成 的 严重 损失 
- 扩展 框架 VBA: 用 户 依据 具体 开发 需求 ， 利 用 VBA 可 改进 和 扩充 RobotStudio 功 能 例如， 外接 插 件 、 宏 处 理 或 定制 用 户 界面 。 
“ 特殊 功能 扩展 : ABB 协 同 合作 伙伴 采用 VBA 已 实现 一 系列 基于 Robot-Studio 的 应 用 开发 ， 使 其 能 够 更 好 地 适用 于 绝 焊 、 弯 板 机 管理 、 点 焊 、 叶 片 研磨 等 特殊 应 用 功能 。 
- 直接 上 传 和 下 载 : 机 器 人 程序 无 需 转换 便 可 直接 下 载 到 实际 机 器 人 系统 ， 该 功能 得 益 于 ABB 独 有 的 ViftualRobot 技 术 。 
2. 机 器 人 运行 控制 软件 RobotWare 


机 器 人 运行 控制 软件 RobotWare 是 ABB 公 司 为 支撑 工业 智能 机 器 人 运行 过 程控 制 和 管理 、 使 机 器 人 更 加 有 成 效 和 降低 运行 成 本 所 开发 的 工业 机 器 人 控制 软件 ，Robot-Ware 的 核心 是 ABB 创 新 设计 的 特 
色 化 机 器 人 语言 一 RAPID， 它 是 一 个 高 度 灵活 的 工业 智能 机 器 人 编程 语言 ， 不 仪 能够 高 效 实现 机 器 人 控制 ， 而 且 能 够 实现 人 机 交互 与 协同 。RobotWare 基 于 准确 路 径 的 先进 移动 技术 可 以 保证 过 程 表现 的 
优化 ， 支 持 人 员 安全 和 错误 恢复 的 特性 。 


RobotWare 的 主要 功能 特点 简介 如 下 。 
. 先进 的 开放 体系 结构 : 其 基本 结构 采用 构件 化 设计 、 全 参数 化 配置 、 脚 本 化 运行 及 高 级 仿真 支持 等 ， 具 有 良好 的 运行 效率 和 扩展 能 力 ， 体 现 了 软件 定义 机 器 的 理念 。 
` 灵活 的 应 用 编程 语言 : RAPID 是 一 种 灵活 的 、 高 层次 的 编程 语言 ， 支 持 所 有 的 ABB 机 器 人 系统 ， 易 于 使 用 ， 在 深层 次 上 允许 创建 高 度 复杂 的 解决 方案 。 


“ 优秀 的 运动 控制 特性 : 运动 控制 是 机 器 人 表现 性 能 的 关键 技术 ， 包 括 路 径 准 确 性 、 速 度 、 周 期 及 外 部 设备 的 可 编程 性 和 同步 。RobotWare 通 过 TrueMove 可 实现 机 器 人 在 任何 速度 下 的 运动 路 径 与 编程 设 
置 的 路 径 一 致 所 具有 的 准确 性 ， 以 及 QuickMove 自 优化 移动 控制 方法 可 保证 每 一 时 刻 的 最 大 加 速度 使 周期 时 间 处 在 最 小 值 状态 ， 从 而 实现 优秀 的 运动 控制 特性 。 


. 健壮 的 系统 适应 能 力 : 在 材料 或 功能 不 能 精确 确定 的 应 用 中 ， 机 器 人 能 够 被 设置 成 软件 伺服 模式 ， 这 就 意味 着 机 器 人 在 遭遇 环境 阻力 时 表现 的 像 一 个 机 械 弹 先 一 样 。 此 外 ， 还 有 安全 性 、 错 误 处 理 等 
设计 。 
广泛 的 外 部 连接 能 力 : RobotWare 不 仅 具有 较 强 的 联网 通信 能 力 ， 而 且 广泛 兼容 各 类 人 机 交互 设备 和 I/O 接 口 形态 。 
3. 升 级 的 机 器 人 控制 软件 RobotWare6 


在 RobotWare 开 放 结 构 和 已 有 功能 的 基础 上 ， 为 了 进一步 扩展 其 功能 ,优化 其 性 能 ，ABB 进 一 步 研发 了 RobotWare6。 其 在 继承 ABB 独 树 一 帜 的 运动 控制 及 安全 性 能 的 基础 上 ， 进 一 步 优化 了 编程 与 控 
制 ， 主 要 体现 在 以 下 几 方 面 。 


- 新 增 安 装 管理 器 (Installation Manager) : 采用 灵活 的 许可 机 制 和 直观 的 全 新 用 户 界面 ， 可 大 幅 缩 短 机 器 人 安装 设置 及 系统 配置 调整 的 时 间 。 
外 部 制导 运动 (Externally Guided Motion) : 操作 员 可 利用 外 部 传感器 实 精准 的 机 器 人 运行 控制 ， 集 成 商 也 可 利用 其 进一步 优化 工作 站 设计 及 生产 流程 。 传 感 器 可 提供 多 种 输入 绝对 位 置 的 途径 ， 


并 具备 路 径 修正 的 能 力 。 路 径 更 新 频率 可 达 4 毫 秒 一 次 ， 控 制 器 滞后 时 间 为 8 一 20 毫 秒 ， 具 体 实 现 取 决 于 机 器 人 型 


.支持 双 辟 机 器 人 的 运动 控制 : ABB 推 出 了 世界 上 首 台 真正 实现 人 机 协作 的 双 辟 机 器 人 YuMi， 它 具有 视觉 和 触 党 ， 双 臂 灵 巧 并 以 软 性 材料 包 训 ， 同 时 配备 创新 的 力 传 感 技术 ， 在 人 机 互动 中 可 以 保障 操 


作 人 员 的 安全 。 


“ 配备 更 为 丰富 的 控制 途径 : 例如 增加 高 级 编程 接口 、 实 现 平板 电脑 与 智能 手机 集成 等 。 


10.1.3 ”服务 机 器 人 软件 体系 框架 SAFSR 


服务 智能 机 器 人 是 一 个 复杂 的 系统 ， 需 要 有 一 个 高 灵活 性 、 可 扩展 性 和 可 维护 性 的 软件 体系 框架 支持 服务 智能 机 器 人 软件 的 设计 与 集成 。 为 此 ，A*STAR 机 器 人 实验 室 提 出 了 一 个 称 为 SAFSR 的 软件 体系 
框架 ， 如 图 10-2 所 示 。 
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图 10-2 ”SAFSR 软 件 结构 


SAFSR 软 件 结构 分 为 四 个 层次 ， 包 括 感知 层 、 执 行 控制 层 、 物 理 层 与 设备 层 。 设 备 层 提供 了 基本 的 基础 设施 以 连接 不 同 的 机 器 人 硬件 和 机 械 组 件 到 物理 层 ; 物理 层 收集 和 分 析 来 自 不 同 硬件 组 件 的 信息 
并 将 分 析 后 的 信息 发 送 到 执行 控制 层 ; 执行 控制 层 实现 物理 层 信息 的 序列 化 并 且 在 考虑 执行 路 径 的 条 件 下 对 其 进行 监督 管理 ， 同 时 也 会 将 相关 的 信息 发 送 到 感知 层 以 便 进一步 处 理 ; 最 后 ， 感 知 层 以 检测 环 
境 模式 和 机 器 人 执行 特定 任务 的 方式 处 理 多 重任 务 。 


SAFSR 整 合并 支持 接近 实时 和 不 确定 动态 真实 世界 环境 下 的 不 同 硬件 和 软件 构件 层次 之 间 的 TCP/IP socket 消 息 通 信 。 所 有 的 构件 均 用 于 完成 特定 的 任务 ， 如 导航 构件 用 于 完成 导航 相关 的 具体 任务 。 


SAFSR 还 有 以 下 几 个 核心 构件 : 关注 直接 对 话 (ADD) 、 统 一 机 器 人 框架 (URF) 和 对 话 管理 系统 (DMS) 。 尽 管 有 些 底层 算法 是 用 C++ 实 现 的 ,但 构件 功能 都 是 用 XML 脚 本 描述 ， 并 遵循 XML 的 语 
法 和 语义 的 ， 以 便 能 构造 一 个 灵活 的 、 可 扩展 、 可 维护 性 的 服务 机 器 人 。 例 如 ， 新 的 特征 与 表现 提升 能 够 容易 地 通过 更 新 或 添加 XML 脚 本 来 实现 ， 而 不 需要 改变 底层 的 C++ 代 码 。 同 时 ， 这 些 改变 或 需求 更 
新 是 在 运行 时 动态 加 载 的 ， 而 不 需要 停止 系统 运行 ， 也 不 必 重 新 编译 模块 的 代码 ， 这 样 就 可 以 在 一 定 程度 上 降低 难度 和 成 本 。 


10.2 ”智能 机 器 人 软件 未 来 的 友 展 万 同 


在 当今 工业 4.0 浪 潮 中 ， 智 能 机 器 人 ， 尤 其 是 工业 智能 机 器 人 ， 在 多 种 先进 技术 的 推动 下 得 以 快速 发 展 ， 其 中 智能 机 器 人 软件 技术 发 展 是 重点 之 一 。 各 种 新 型 计算 技术 、 信 息 处 理 技术 与 智能 机 器 人 领域 
相 结合 ， 推 动 智能 机 器 人 软件 的 发 展 。 智 能 机 器 人 软件 不 仅 具 有 多 学 科 融 合 的 特点 ， 涉 及 计算 技术 、 自 动 控制 、 机 电 一 体 化 、 人 工 智能 等 ， 而 且 具 有 软 硬 件 融 合 特性 ， 以 及 实时 性 、 安 全 性 、 可 靠 性 等 特 
性 ， 因 此 ， 智 能 机 器 人 的 设计 、 实 现 和 验证 面临 着 新 的 技术 挑战 。 


智能 机 器 人 软件 的 主要 发 展 方向 体现 在 下 述 几 个 方面 。 


10.2.1 ”多 传感器 信息 融合 


随 着 机 器 人 智能 程度 的 不 断 提 高 ， 需 要 获取 的 环境 信息 也 越 来 越 多 ， 单 一 的 传感器 已 难以 感知 全 面 而 精确 的 环境 与 状态 。 单 一 传感器 获得 的 信息 非常 有 限 而 且 还 受 自身 品质 和 性 能 的 影响 ， 因 此 ， 智 能 
机 器 人 通常 配 有 数量 众多 的 不 同类 型 的 传感器 以 满足 探测 和 数据 采集 的 需要 。 一 人 台 智 能 机 器 人 可 能 配备 几 十 乃 全 近 上 百 个 传感器 。 若 对 各 传感器 采集 的 信息 进行 单独 孤立 地 处 理 ， 不 仅 会 导致 信息 处 理 量 的 增 
加 ， 而 且 还 会 割断 各 传感器 信息 间 的 内 在 联系 ， 丢 失 经 过 有 机 组 合 后 可 能 列 含 的 环境 或 行为 特征 ， 造 成 信息 资源 的 浪费 。 多 传感器 信息 融合 技术 可 有 效 地 解决 上 述 问题 。 它 综合 运用 控制 原理 、 信 号 处 理 、 
仿生 学 、 人 工 智 能 和 数理 统计 等 理论 ， 将 分 布 在 不 同位 置 、 处 于 不 同 状态 的 多 传感器 所 提供 的 局 部 的 、 不 完整 的 观察 量 加 以 综合 ， 消 除 多 传感器 信息 之 间 可 能 人 存在 的 见 余 和 矛盾 ， 利 用 信息 互补 ， 降 低 不 确 
定性 ， 以 形成 对 系统 环境 相对 完整 一 致 的 感知 描述 ， 从 而 提高 智能 机 器 人 决策 和 规划 的 科学 性 、 反 应 的 快速 性 和 正确 性 ， 因 此 ， 机 器 人 多 传感器 信息 融合 技术 已 成 为 智能 机 器 人 研究 领域 的 关键 技术 之 一 。 


智能 机 器 人 多 传感器 信息 融合 需要 研究 的 较 多 ， 主 要 包含 以 下 几 个 方面 。 


1) 微型 化 和 智能 化 机 器 人 传感器 。 这 是 机 器 人 多 传感器 信息 融合 技术 的 硬件 基础 。 为 进一步 减 小 机 器 人 的 体积 并 提高 传感器 的 性 能 ， 机 器 人 传感器 将 向 着 信息 转换 、 处 理 、 传 输 为 一 体 的 智能 化 方向 发 
展 。 


2) 适合 智能 机 器 人 的 多 传感器 信息 融合 先进 算法 。 目 前 主要 采用 几 种 算法 共同 使 用 的 方法 弥补 单一 多 传感器 信息 融合 算法 的 缺陷 和 局 限 性 ， 将 模糊 逻辑 、 神 经 网 络 、 进 化 计算 、 小 波 变换 等 智能 计算 方 
法 有 机 地 结合 在 一 起 是 一 个 重要 的 发 展 趋势 。 多 种 信息 融合 算法 综合 运用 的 同时 增加 了 系统 的 计算 量 ， 难 以 满足 系统 的 实时 性 要 求 ， 因 此 ， 迫 切 需 要 机 器 人 多 信息 融合 算法 创新 。 例 如 ， 采 用 在 线 学 习 模型 
方法 融合 来 自 于 激光 探测 仪 和 红外 辐 度 探测 器 的 信息 ， 以 提高 机 器 人 的 导航 能 力 ; 研究 基于 任务 的 神经 网 络 多 传感器 数据 融合 方法 ， 根 据 机 器 人 的 不 同 任务 ， 利 用 优化 神经 网 络 算法 对 多 种 传感器 信息 进行 
融合 决策 。 


3) 综合 化 多 传感器 管理 。 信 息 融 合 技术 与 机 器 人 多 源 或 多 类 传感器 相 结合 ， 构 成 从 上 而 下 的 层次 化 机 器 人 多 传感器 信息 融合 系统 ， 其 能 够 在 同一 时 刻 或 同一 时 间 段 实现 多 种 任务 功能 。 但 在 不 同 的 时 间 
周期 内 ， 系 统 任务 可 能 不 同 、 任 务 的 性 能 要 求 也 有 差异 ， 这 就 需要 多 传感器 的 实时 管理 ， 支 持 自 适 应 信息 融合 ， 实 现 多 传感器 信息 融合 系统 综合 化 。 多 传感器 管理 不 仅 可 实现 信息 融合 的 协调 控制 ， 同 时 还 
应 实现 系统 自 组 织 优化 能 力 。 


4) 信息 融合 仿生 机 理 。 对 感官 获得 的 信息 进行 融合 是 自然 界 中 动物 的 本 能 。 动 物 能 够 自然 地 运用 这 一 功能 把 来 自 于 不 同感 官 的 信息 ， 通 过 大 脑 综合 、 分 析 、 处 理 以 实现 准确 识别 ， 并 利用 先 验 知识 佑 
计 、 理 解 周围 环境 和 正在 发 生 的 事件 。 对 动物 视觉 、 听 觉 、 触 觉 等 感知 功能 和 信息 融合 方式 的 研究 将 有 助 于 工程 问题 的 解决 。 例 如 ， 有 人 对 昨 星 的 凝视 机 理 、 密 蜂 飞 过 管道 的 导航 行为 及 采 密 时 的 定 心 能 力 
进行 研究 ， 分 别 得 出 了 物体 学 围 确定 方法 、 狭 长 通道 导航 和 障碍 密集 环境 中 的 路 径 规 划算 法 ; 从 蝙蝠 、 海 豚 等 动物 利用 超声 波 定位 和 避 障 中 得 到 启示 ， 实 现 了 物体 的 三 维 定位 。 因 此 ， 加 强 信息 仿生 机 理 的 


研究 将 会 使 机 器 人 多 信息 融合 取得 突破 性 的 发 展 。 


10.2.2 人 机 协作 


人 机 协作 是 智能 机 器 人 发 展 的 新 形态 。 其 本 质 是 将 机 器 人 的 准确 性 和 高 效率 与 人 的 智慧 与 灵活 性 相 结合 ， 共 同 高 效率 高 质量 地 完成 特定 作业 。 可 以 预见 在 未 来 ， 智 能 机 器 人 将 作为 人 类 的 同事 和 合作 者 
出 现在 多 个 领域 。 具 有 人 机 协同 能 力 的 智能 机 器 人 ， 我 们 可 称 之 为 协作 机 器 人 。 例 如 入 机 协同 装配 中 ， 机 器 人 能 够 迅速 调整 自己 的 工作 计划 以 适应 不 断 变化 的 生产 情况 ， 响 应 生产 工人 通过 和 触摸、 声音 或 手 
势 下 达 的 命令 。 机 器 人 与 工人 在 生产 线 上 协同 工作 可 以 减少 交 货 时 间 ， 其 理念 是 将 机 器 人 的 准确 性 与 人 的 灵活 性 相 结合 ， 既 保证 了 就 业 ， 又 提高 了 生产 效率 ， 并 使 得 生产 系统 更 强大 、 更 灵活 。 机 器 人 帮 人 
完成 重 物 提 升 等 操作 ， 例 如 机 器 人 可 以 把 一 个 发 动机 体 搬 到 工人 指定 的 任何 位 置 ， 从 而 减轻 工人 不 必要 的 体力 劳动 ， 通 过 承担 工厂 中 的 体力 活动 ， 协 作 机 器 人 可 以 为 人 们 创造 更 多 样 的 展示 能 力 的 机 会 ， 而 
且 身 体 状况 、 性 别 或 年 龄 都 将 变 得 不 再 重要 。 


协作 机 器 人 可 用 在 个 性 化 、 小 规模 、 变 动 频繁 的 小 型 生产 线 或 人 机 混 线 的 半自动 生产 环境 中 。 因 为 ， 相 对 于 传统 机 器 人 固化 、 适 应 性 差 等 特点 ， 人 机 协作 机 器 人 能 够 实现 定制 化 生产 ， 满 足 多 品种 、 小 
批量 、 柔 性 、 快 速 等 传统 工业 机 器 人 难以 涉及 的 新 型 制造 模式 。 


人 机 协作 可 由 机 器 实现 简单 的 操作 ， 并 运行 底层 的 自动 化 控制 ， 而 “人 ”来 实现 上 层 的 监督 控制 、 任 务 安排 、 轨 人 迹 设 定 和 相关 操作 等 。 既 然 是 人 机 协作 ， 那 么 整个 任务 必然 是 由 “人 ”与 “机 器 ”协同 
完成 的 。 既 然 是 协同 完成 任务 ， 那 么 就 需要 对 任务 有 明确 的 分 工 。 在 人 机 协作 系统 中 ， 人 主要 负责 “定性 ”判断 决策 ， 而 机 器 则 负责 “定量 ”计算 推理 ， 二 者 相 结合 ， 缺 一 不 可 。 


人 机 协作 的 深层 内 涵 是 “人 机 智能 融合 ”， 它 代表 “人 ”与 “机 器 ”需要 共同 完成 指定 的 任务 ， 在 完成 任务 的 过 程 中 二 者 的 行为 还 要 保持 步调 一 致 ， 并 且 两 者 在 意识 层面 还 需要 始终 协调 。 而 这 需要 智 
能 机 器 人 必须 进一步 提升 机 器 感 务 、 人 机 共 融 等 多 项 技术 水 平 。 


要 实现 人 机 共 融 ， 就 需要 制定 机 器 人 安全 操作 的 新 标准 ， 研 究 无 围栏 环境 下 人 与 机 器 人 肩 并 肩 协同 工作 的 相关 技术 ， 使 得 智能 机 器 人 及 时 感受 人 类 同事 的 存在 ， 并 且 吉 免 安 全 事故 的 发 生 ; 需要 依据 具 
体 领 域 和 应 用 场景 ， 确 定 科 学 的 人 机 分 工 与 协作 任务 ; 确定 适应 人 机 协同 的 控制 与 管理 策略 及 其 实现 机 制 ;必须 解决 好 人 机 交互 问题 ， 以 便 建 立 一 个 能 够 实现 “人 ”与 “机 器 ”信息 传输 的 渠道 ， 如 在 人 机 
协作 机 器 人 中 用 阻抗 控制 感受 力 觉 ， 使 人 能 够 感知 机 器 人 与 加 工 部 件 之 间 的 机 械 接触 。 同 时 ， 需 要 考虑 多 通道 人 机 交互 模式 的 整合 ， 通 过 几 种 效应 通道 的 协作 ， 减 少 单个 通道 的 工作 负担 ， 提 高 人 机 交互 的 
可 靠 性 和 工作 效率 。 


10.2.3 ”人 工 智 能 深化 应 用 


近年 来 ， 人 工 智 能 (Al) 领域 发展 飞速 ， 人 工 智能 企图 了 解 智能 的 实质 ， 并 产生 出 能 以 人 类 智能 相似 的 方式 作出 反应 的 智能 机 器 。 将 人 工 智 能 应 用 到 机 器 人 领域 ， 能 够 有 效 地 提高 机 器 人 的 智能 程度 。 
有 具体 来 说 ， 通 过 人 工 智能 可 以 有 效 地 提高 机 器 人 的 自然 语言 理解 、 语 音 识别 、 人 脸 识 别 和 跟踪 、 物 体 识别 、 体 感 交 互 、 室 内 定位 和 导航 视觉 等 多 种 智能 人 机 交互 能 力 。 因 此 ， 人 工 智 能 是 智能 机 器 人 发 展 的 
核心 推动 力 。 


人 工 智能 在 智能 机 器 人 领域 的 应 用 可 分 为 对 “外 ”和 对 “内 ”两 个 方面 。 对 外 是 指 对 外 界 信息 的 正确 获取 ， 探 测 与 感知 技术 是 关键 ,将 涉及 声音 识别 、 图 像 识 别 等 感知 方法 ,探测 与 感知 的 有 效 性 和 真 
实 性 直接 决定 了 Al 的 智能 程度 ， 也 就 直接 决定 了 未 来 智能 机 器 人 人 机 交互 的 体验 和 准确 度 。 对 内 是 指 自 我 学 习 与 优化 ， 即 将 外 界 所 获取 的 信息 和 训练 学 习 所 得 到 的 信息 进行 快速 匹配 ， 及 时 做 出 正确 决策 。 
高 性 能 算法 和 高 性 能 、 低 能 耗 硬件 是 关键 的 前 提 条 件 ， 其 决定 了 未 来 智能 机 器 人 的 数据 获取 、 智 能 处 理 与 环境 适应 能 力 。 


就 智能 机 器 人 软件 发 展 而 言 ， 人 工 智能 主要 体现 在 以 下 几 类 算法 及 其 软件 的 实现 上 。 


1) 机 器 视觉 : 机 器 视觉 是 智能 机 器 人 发 展 的 一 个 关键 因素 。 其 最 初 是 作为 机 器 人 的 辅助 工具 ， 提 高 柔性 和 对 工作 环境 的 反馈 ， 主 要 应 用 于 引导 、 定 位 、 检 测 和 识别 等 ， 随 着 工业 大 数据 和 深度 学 习 的 发 
展 ， 未 来 将 使 机 器 视觉 成 为 智能 生产 系统 的 主导 ， 以 便 做 出 决策 和 预 判断 。 


2) 深度 学 习 : 深度 学 习 可 推动 机 器 人 摆脱 预 编程 序 的 束缚 ， 真 正 走 向 智能 化 。 深 度 学 习 使 机 器 人 可 以 像 人 一 样 通过 学 习 掌 握 新 的 技能 ， 适 应 未 知 的 工作 环境 。 深 度 学 习 在 工业 机 器 人 的 应 用 方面 分 为 以 


下 三 个 层次 : 机 器 人 通过 试 错 学 会 新 技能 ;多 人 台 共 享 经 验 提 高 学 习 效率 ; 机 器 人 预防 并 且 自 行 修复 故障 。 目 前 已 经 达到 第 二 个 层次 。 


3) 多 模 态 交互 : 未 来 智能 机 器 人 将 会 更 好 地 与 人 类 进行 多 模 态 交 豆 。 除 了 丰富 的 人 机 区 互 硬件 之 外 ， 其 更 多 地 体现 在 思维 强化 引擎 、 情 感 计算 引擎 及 自学 习 引 警方 面 。 思 维 模式 引擎 是 通过 多 种 方法 对 
逆向 思维 、 推 理 思维 等 思维 模式 进行 有 效 的 训练 ， 使 机 器 人 能 够 学 习 人 类 的 思维 模式 乃至 思维 能 力 机 制 ;情感 计算 引擎 主要 包含 情感 识别 及 情感 表达 ， 以 帮助 智能 机 器 人 通过 多 维度 输出 表情 及 动作 等 ; 自 
学 习 引 警 则 是 基于 自学 习 算 法 和 大 数据 支持 ， 在 高 性 能 硬件 上 实现 快速 迭代 ， 以 提高 智能 机 器 人 的 智能 水 平 。 


10.2.4 ”多 机 器 人 协作 


多 机 器 人 系统 是 指 通 过 组 织 多 台 智 能 机 器 人 协作 完成 某 一 任务 的 机 器 人 群体 。 协 作 性 是 多 机 器 人 系统 的 重要 特征 和 关键 指标 。 通 过 适当 的 协作 机 制 ， 多 机 器 人 系统 可 以 获得 系统 级 非 线 性 功能 增 量 ， 从 
而 突破 单机 器 人 系统 在 感知 、 决 策 及 执行 能 力 等 方面 受到 的 限制 ， 从 本 质 上 提升 系统 性 能 ， 甚 至 完成 单个 机 器 人 无 法 实现 的 任务 。 此 外 ， 相 对 于 单机 器 人 系统 ， 多 机 器 人 系统 拥有 时 间 、 空 间 、 功 能 、 信 息 
和 资源 上 的 分 布 特性 ， 从 而 在 任务 适用 性 、 健 壮 性 、 可 扩张 性 等 方面 表现 出 潜在 的 优越 性 ， 未 来 多 机 器 人 协作 系统 将 会 有 广阔 的 发 展 应 用 前 景 。 


例如 ， 智 能 工厂 构建 中 ， 必 将 应 用 多 类 型 、 多 能 力 的 工业 智能 机 器 人 构建 面向 一 类 智能 产品 的 柔性 生产 线 ， 以 支撑 实现 个 性 化 高 品质 智能 产品 的 高 效率 制造 。 这 就 使 得 异 构 工 业 智能 机 器 人 协作 成 为 必 
须 解 决 的 关键 问题 和 核心 技术 。 其 难度 在 于 不 仅 单 一 工业 智能 机 器 人 具有 信息 物理 深度 融合 及 自主 执行 等 特性 ， 而 且 多 类 型 、 多 能 力 的 工业 智能 机 器 人 还 表现 在 结构 、 能 力 及 交互 的 异 构 性 ， 并 且 要 面 对 不 
同 柔 性 的 生产 过 程 、 可 变 生产 流程 及 个 性 产品 制造 等 需求 ， 使 得 异 构 工业 智能 机 器 人 协同 语义 难以 统一 ， 协 同 策略 不 可 唯一 ， 协 同 机 制 必 须 灵活 ， 必 须 深化 研究 异 构 工业 智能 机 器 人 自 适 应 协同 策略 与 机 
制 ， 研 发 出 相应 的 算法 和 软件 ， 才 能 支撑 柔性 生产 线 构建 及 智能 工厂 的 实现 。 


以 工业 应 用 为 例 ， 多 智能 机 器 人 协作 研究 及 其 软件 实现 主要 包含 以 下 几 方 面 。 


1) 异 构 工业 智能 机 器 人 协同 语义 及 其 规范 : 如 何 合理 地 定义 和 规范 描述 异 构 工业 智能 机 器 人 的 协作 语义 是 首要 解决 的 关键 问题 。 在 深入 分 析 典 型 工业 智能 机 器 人 能 力 及 柔性 生产 需求 的 基础 上 ， 需 要 定 
义 异 构 工业 智能 机 器 人 协作 语义 ， 提 出 相应 的 技术 规范 。 


2) 多 智能 机 器 人 协作 策略 及 其 机 制 实现 : 怎样 保障 异 构 工业 智能 机 器 人 协作 的 适应 性 与 高 效 性 是 多 智能 机 器 人 协作 研究 的 另 一 个 关键 问题 。 需 要 分 析 总 结 不 同类 型 的 协作 需求 ， 建 立 适应 不 同 应 用 场景 
的 协作 模型 ， 提 出 多 智能 机 器 人 协作 策略 集 ; 需要 分 析 不 同 技术 实现 途径 的 优 缺 点 ， 基 于 主流 工业 网 络 ， 设 计 自 适 应 协作 机 制 ， 自 主 研发 相应 软件 。 


3) 典型 智能 生产 线 试验 床 综合 验证 : 如 何 有 效 验证 多 智能 机 器 人 协作 的 效果 与 性 能 ， 是 需要 解决 的 第 三 个 天 键 问题 。 需 要 采用 虚拟 设计 与 仿真 方法 构建 典型 智能 产品 柔性 生产 线 试 验 床 ， 仿 真 真实 的 多 
智能 机 器 人 协作 生产 过 程 ， 在 此 环境 下 ， 对 提出 的 协作 模型 、 策 略 和 机 制 进行 全 面 的 验证 和 定量 评价 。 


10.3 云 机 器 人 软件 平台 


10.3.1 云 机 器 人 发 展 背 景 


云 机 器 人 (Cloud Robotic) 的 概念 由 James Kuffner 博 士 于 2010 年 在 Humanoids2010 会 议 上 提出 的 ， 当 初 是 针对 Google 无 人 驾驶 车 而 提出 这 一 概念 。2011 年 欧盟 资助 了 一 个 RoboEarth 项 目 ， 该 项 

旨 在 让 机 器 人 可 以 通过 互联 网 分 享 知 识 ， 并 且 能 够 获取 强大 的 机 器 人 云 服 务 。 以 往 的 机 器 人 ， 人 往往 只 能 依靠 编程 完成 特定 的 任务 ， 借 助 于 人 类 处 理 过 的 信息 ， 完 成 精确 指令 和 任务 ， 在 没有 相关 程序 支持 

的 情况 下 ， 就 会 举步维艰 。 而 云 机 器 人 借助 于 网 络 和 云 计算 ， 帮助 机 器 人 相互 学 习 、 共 享 知 识 ， 这 不 仅 能 够 降低 成 本 ， 还 会 帮助 机 器 人 提高 自学 能 力 、 适 应 能 力 ， 推 动 更 快 更 大 规模 的 普及 。 云 机 器 人 的 这 
些 能 力 提高 了 其 对 复杂 环境 的 适应 性 ， 云 机 器 人 也 必 将 成 为 机 器 人 未 来 的 发 展 趋势 。 融 入 云 计算 领域 的 云 机 器 人 将 更 轻装 、 更 便宜 、 更 智能 ， 对 网 络 和 自动 化 的 每 一 个 领域 和 行业 的 影响 将 会 无 比 深远 。 


云 机 器 人 是 由 遥 操作 机 器 人 、 网 络 机 器 人 发 展 而 来 的 。 遥 操作 机 器 人 技术 是 指 操作 人 员 监 视 和 控制 远方 机 器 人 完成 各 种 作业 ， 从 而 使 机 器 人 能 够 代替 人 类 在 一 些 无 法 触及 的 ， 甚 至 一 些 危及 人 类 健康 或 
安全 的 环境 下 完成 各 种 任务 。 网 络 机 器 人 技术 则 是 把 标准 通信 协议 和 标准 人 机 接口 作为 基本 设施 ， 再 将 它们 与 有 实际 观测 操作 技术 的 机 器 人 融合 在 一 起 ， 即 可 实现 无 论 何 时 何 地， 无 论 是 谁 都 能 使 用 的 远程 
环境 观测 操作 系统 。 随 着 网 络 机 器 人 技术 的 发 展 成 熟 ， 更 具 适 应 能 力 、 更 具 智 能 化 的 云 机 器 人 技术 随 之 而 出 。 目 前 ， 云 机 器 人 与 网 络 机 器 人 (包括 遥 操 作 和 多 机 器 人 系统 ) 都 是 通过 网 络 使 得 机 器 人 分 享 信 
息 ， 进 行 扩展 。 但 云 机 器 人 区 别 于 网 络 机 器 人 之 处 在 于 : 其 一 ， 在 云端 动态 地 计算 ， 为 机 器 人 分 配 资源 ， 机 器 人 获得 的 资源 具有 弹性 ， 能 按 需 服务 ; 其 二 ， 云 机 器 人 是 “大 脑 在 云端 ”， 机 器 人 通过 网 络 获 
取 处 理 后 的 结果 ， 网 络 机 器 人 的 处 理 还 是 在 本 体 。 


随 着 云 计 算 技 术 的 发 展 以 及 智能 机 器 人 ， 尤 其 是 智能 机 器 人 集群 的 应 用 需求 ， 云 计算 与 智能 机 器 人 的 结合 所 形成 的 云 机 器 人 具有 广 阅 的 应 用 前 景 。 例 如 ， 未 来 智能 工厂 需要 实现 全 自动 化 的 定制 化 生产 
线 ， 多 智能 机 器 人 和 其 他 智能 机 器 实现 快速 重 配 置 和 动态 重 构 ， 利 用 云 计 算 的 丰富 资源 和 按 需 服务 能 力 不 仅 有 效 地 解决 由 于 机 器 人 自身 处 理 、 存 储 能 力 及 能 源 等 限制 所 带 来 的 问题 ， 机 器 人 可 以 将 计算 复杂 
的 任务 在 云端 执行 ， 同 时 云端 也 可 以 为 机 器 人 提供 大 量 的 知识 集 ， 提 高 智能 机 器 人 的 性 能 ， 而 且 能 够 支持 智能 生产 线 的 高 效 实现 。 云 机 器 人 将 在 柔性 装配 、 空 间 监 控 、 无 人 驾驶 、 物 流 管理 、 智 能 看 护 、 远 
程 手 术 等 方面 得 到 应 用 。 如 图 10-3 所 示 。 





图 10-3 云 机 器 人 应 用 模式 


具体 来 说 ， 云 机 器 人 将 会 对 智能 机 器 人 的 发 展 带 来 以 下 好 处 。 
(1) ERA 


将 计算 密集 型 和 数据 密集 型 任务 放 在 云端 执行 ， 缓 解 机 器 人 端的 运算 负荷 。 例 如 移动 机 器 人 导航 、 快 速 地 图 重建 、 抓 取 规 划 、 路 径 规 划 等 计算 量 比较 大 的 应 用 场景 ， 通 过 云 平台 就 可 以 有 效 地 减少 机 器 
人 端的 计算 量 


o 


(2) 共享 知识 集 


通过 云 平台 ， 可 以 实现 不 同 机 器 人 系统 之 间 的 资源 共享 。 例 如 ，Columbia Grasp 数 据 集 是 一 个 预先 计算 好 的 基于 3D 模 型 的 抓 取 数 据 集 ， 目 前 已 经 应 用 在 很 多 研究 学 习 中 ， 欧 共 体 的 RoboEarth 项 目 开 
发 了 一 个 基于 Web 的 知识 集 ， 通 过 它 ， 机 器 人 可 以 共享 已 经 收集 和 获得 的 信息 。 


(3) 集体 学 习 


云 平台 为 网 络 化 机 器 人 提供 了 一 个 共享 数据 与 处 理 信息 的 优秀 平台 ， 其 可 支持 智能 机 器 人 通过 各 种 机 器 学 习 方 法 提高 认 知 能 力 与 智能 水 平 。 集 体 学 习 的 一 个 简单 例子 是 灾难 恢复 场景 构建 ， 在 该 场景 中 
各 种 移动 机 器 人 被 部 署 以 建立 一 个 位 置地 图 ， 多 机 器 人 通过 云 平台 服务 实现 协同 ， 同 时 发 送 可 以 被 其 他 机 器 人 共享 的 位 置信 息 ， 从 而 强化 地 图 构建 。 


10.3 云 机 器 人 软件 平台 


10.3.1 云 机 器 人 发 展 背 景 


云 机 器 人 (Cloud Robotic) 的 概念 由 James Kuffner 博 士 于 2010 年 在 Humanoids2010 会 议 上 提出 的 ， 当 初 是 针对 Google 无 人 驾驶 车 而 提出 这 一 概念 。2011 年 欧盟 资助 了 一 个 RoboEarth 项 目 ， 该 项 
目 旨 在 让 机 器 人 可 以 通过 互联 网 分 享 知 识 ， 并 且 能 够 获取 强大 的 机 器 人 云 服 务 。 以 往 的 机 器 人 ， 往 往 只 能 依靠 编程 完成 特定 的 任务 ， 借 助 于 人 类 处 理 过 的 信息 ， 完 成 精确 指令 和 任务 ， 在 没有 相关 程序 支持 
的 情况 下 ， 就 会 举步维艰 。 而 云 机 器 人 借助 于 网 络 和 云 计算 ， 帮助 机 器 人 相互 学 习 、 共 享 知 识 ， 这 不 仅 能 够 降低 成 本 ， 还 会 帮助 机 器 人 提高 自学 能 力 、 适 应 能 力 ， 推 动 更 快 更 大 规模 的 普及 。 云 机 器 人 的 这 
些 能 力 提高 了 其 对 复杂 环境 的 适应 性 ， 云 机 器 人 也 必 将 成 为 机 器 人 未 来 的 发 展 趋势 。 融 入 云 计算 领域 的 云 机 器 人 将 更 轻装 、 更 便宜 、 更 智能 ， 对 网 络 和 自动 化 的 每 一 个 领域 和 行业 的 影响 将 会 无 比 深远 。 


云 机 器 人 是 由 遥 操作 机 器 人 、 网 络 机 器 人 发 展 而 来 的 。 遥 操作 机 器 人 技术 是 指 操作 人 员 监 视 和 控制 远方 机 器 人 完成 各 种 作业 ， 从 而 使 机 器 人 能 够 代替 人 类 在 一 些 无 法 触及 的 ， 甚 至 一 些 危及 人 类 健康 或 
安全 的 环境 下 完成 各 种 任务 。 网 络 机 器 人 技术 则 是 把 标准 通信 协议 和 标准 人 机 接口 作为 基本 设施 ， 再 将 它们 与 有 实际 观测 操作 技术 的 机 器 人 融合 在 一 起 ， 即 可 实现 无 论 何 时 何 地 ， 无 论 是 谁 都 能 使 用 的 远程 
环境 观测 操作 系统 。 随 着 网 络 机 器 人 技术 的 发 展 成 熟 ， 更 具 适 应 能 力 、 更 具 智 能 化 的 云 机 器 人 技术 随 之 而 出 。 目 前 ， 云 机 器 人 与 网 络 机 器 人 (包括 遥 操 作 和 多 机 器 人 系统 ) 都 是 通过 网 络 使 得 机 器 人 分 享 信 
息 ， 进 行 扩展 。 但 云 机 器 人 区 别 于 网 络 机 器 人 之 处 在 于 : 其 一 ， 在 云端 动态 地 计算 ， 为 机 器 人 分 配 资源 ， 机 器 人 获得 的 资源 具有 弹性 ， 能 按 需 服务 ; 其 二 ， 云 机 器 人 是 “大 脑 在 云端 ”， 机 器 人 通过 网 络 获 
取 处 理 后 的 结果 ， 网 络 机 器 人 的 处 理 还 是 在 本 体 。 


随 着 云 计 算 技 术 的 发 展 以 及 智能 机 器 人 ， 尤 其 是 智能 机 器 人 集群 的 应 用 需求 ， 云 计算 与 智能 机 器 人 的 结合 所 形成 的 云 机 器 人 具有 广 阅 的 应 用 前 景 。 例 如 ， 未 来 智能 工厂 需要 实现 全 自动 化 的 定制 化 生产 
线 ， 多 智能 机 器 人 和 其 他 智能 机 器 实现 快速 重 配置 和 动态 重 构 ， 利 用 云 计 算 的 丰富 资源 和 按 需 服 务 能 力 不 仅 有 效 地 解决 由 于 机 器 人 自身 处 理 、 存 储 能 力 及 能 源 等 限制 所 带 来 的 问题 ， 机 器 人 可 以 将 计算 复杂 
的 任务 在 云端 执行 ， 同 时 云端 也 可 以 为 机 器 人 提供 大 量 的 知识 集 ， 提 高 智能 机 器 人 的 性 能 ， 而 且 能 够 支持 智能 生产 线 的 高 效 实现 。 云 机 器 人 将 在 柔性 装配 、 空 间 监 控 、 无 人 驾驶 、 物 流 管理 、 智 能 看 护 、 远 
程 手术 等 方面 得 到 应 用 。 如 图 10-3 所 示 。 





图 10-3 云 机 器 人 应 用 模式 


具体 来 说 ， 云 机 器 人 将 会 对 智能 机 器 人 的 发 展 带 来 以 下 好 处 。 
(1) ERTE 


将 计算 密集 型 和 数据 密集 型 任务 放 在 云端 执行 ， 绥 解 机 器 人 端的 运算 负荷 。 例 如 移动 机 器 人 导航 、 快 速 地 图 重建 、 抓 取 规 划 、 路 径 规 划 等 计算 量 比 较 大 的 应 用 场景 ， 通 过 云 平台 就 可 以 有 效 地 减少 机 器 
人 端的 计算 量 。 


(2) 共享 知识 集 


通过 云 平台 ， 可 以 实现 不 同 机 器 人 系统 之 间 的 资源 共享 。 例 如 ，Columbia Grasp 数 据 集 是 一 个 预先 计算 好 的 基于 3D 模 型 的 抓 取 数 据 集 ， 目 前 已 经 应 用 在 很 多 研究 学 习 中 ; 欧 共 体 的 RoboEarth 项 目 开 
发 了 一 个 基于 Web 的 知识 集 ， 通 过 它 ， 机 器 人 可 以 共享 已 经 收集 和 获得 的 信息 。 


(3) 集体 学 习 


云 平台 为 网 络 化 机 器 人 提供 了 一 个 共享 数据 与 处 理 信息 的 优秀 平台 ， 其 可 支持 智能 机 器 人 通过 各 种 机 器 学 习 方 法 提高 认 知 能 力 与 智能 水 平 。 集 体 学 习 的 一 个 简单 例子 是 灾难 恢复 场景 构建 ， 在 该 场景 中 
各 种 移动 机 器 人 被 部 署 以 建立 一 个 位 置地 图 ， 多 机 器 人 通过 云 平台 服务 实现 协同 ， 同 时 发 送 可 以 被 其 他 机 器 人 共享 的 位 置信 息 ， 从 而 强化 地 图 构建 。 


10.3.2 云 机 器 人 系统 结构 与 计算 模型 


1. 云 机 器 人 系统 结构 


云 机 器 人 系统 不 同 于 通常 的 云端 融合 系统 ， 这 是 由 于 智能 机 器 人 自身 特点 、 服 务 对 象 的 物理 特性 以 及 实时 性 与 可 靠 性 等 可 信 要 求 更 加 严格 所 致 。 这 就 使 得 云 机 器 人 系统 结构 也 不 同 于 一 般 的 云端 融合 系 
统 。 近 年 来 ， 云 机 器 人 研究 已 引起 学 术 界 和 科技 界 的 关注 ， 也 出 现 了 多 种 云 与 机 器 人 结合 的 架构 ， 以 下 重点 介绍 两 种 主流 结构 。 


(1) 基于 机 器 人 服务 的 云 结构 


在 该 框架 中 ， 将 不 同 的 智能 机 器 人 及 其 能 力 抽象 为 各 个 云 服务 ， 依 据 应 用 任务 需求 ， 云 系统 选择 合适 的 智能 机 器 人 服务 。 其 关键 机 制 是 智能 机 器 人 的 功能 分 配 ， 系 统 发 现 分 布 式 的 智能 机 器 人 资源 ， 并 
将 请 求 任务 分 配给 适合 的 智能 机 器 人 处 理 。 如 图 10-4 所 示 的 是 一 个 Jeeves 系 统 结 构 ， 该 系统 中 包括 机 器 人 与 互联 网 服务 ， 为 机 器 人 提供 了 一 个 御 载 计算 的 云 计算 平台 。 通 过 云 计 算 与 机 器 人 的 组 合 不 仅 提 供 
了 新 型 的 云 服务 能 力 ， 也 改善 了 机 器 人 本 身 的 各 种 局 限 性 ， 从 而 在 某 种 程度 上 实现 了 智能 机 器 人 的 远程 大 脑 。 
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图 10-4 Jeeves 系 统 结构 


其 中 ，RSNP (Robot Service Network Protocol) 机 器 人 服务 网 络 协议 是 由 机 器 人 服务 计划 (RSI) 发 布 的 基于 Web 服 务 技术 的 标准 化 通信 协议 ， 该 协议 便于 机 器 人 服务 和 互联 网 服务 的 集成 。 
(2) M2M/M2C 结 构 


M2M/M2C 结 构 由 两 层 组 成 : 机 器 到 机 器 层 (M2M) 和 机 器 到 云层 (M2C) 。M2M 层 表示 智能 机 器 人 会 创建 一 个 协作 云 计 算 组 (ad-hoc 云 ) ， 人 允许 智能 机 器 人 分 享 密集 型 计算 任务 、 共 享 资源 、 交 
换 信 息 ， 从 而 更 好 地 实现 协作 。 更 重要 的 是 ， 通 过 接 入 云 可 以 和 不 在 ad-hoc 云 中 的 其 他 智能 机 器 人 进行 通信 。 在 M2C 层 ， 可 以 按 需 为 智能 机 器 人 提供 计算 资源 和 存储 资源 ， 使 得 M2M 层 智能 机 器 人 可 以 使 
用 超过 它们 共享 资源 之 外 的 资源 。 该 系统 可 以 划分 为 两 个 子 系 统一 一 通信 框架 和 灵活 的 计算 架构 。 通 信 采 用 随机 游 走 协议 ,该 协议 在 网 络 状 况 不 好 的 情况 下 只 需要 很 少 的 额外 计算 和 存储 资源 ， 这 将 超越 使 
用 云 基础 设施 作为 中 央 M2M/M2C 超 级 节点 的 通信 网 络 。 灵 活 计算 架构 是 通过 建立 多 种 云 机 器 人 模型 ， 根 据 网 络 状 况 、 应 用 需求 和 资源 状况 选择 合适 的 模型 。 图 10-5 是 该 框架 的 一 个 结构 图 。 


<----+> Machine-to-Cloud (M2C) 
—— Machine-to-Machine (M2C) 





图 10-5 M2M/M2c 2574 
2. 云 机 器 人 计算 模型 
智能 机 器 人 与 云 平台 可 构成 不 同 的 计算 模型 ， 主 要 有 以 下 3 种 典型 模型 。 
(1) 对 等 计算 模型 


如 图 10-6 所 示 ， 每 一 个 智能 机 器 人 或 者 云 平台 中 的 虚拟 设备 都 被 认为 是 一 个 计算 单元 。 智 能 机 器 人 和 虚拟 设备 组 成 一 个 完全 分 布 式 计 算 网 格 。 一 个 复杂 应 用 任务 可 被 分 解 为 若干 小 任务 ， 从 而 在 计算 网 
格 中 的 一 个 子 集 上 执行 。 该 模型 中 的 智能 机 器 人 具有 较 强 的 移动 性 ， 智 能 机 器 人 可 人 在 云 平台 履 盖 的 范围 内 任意 活动 。 





图 10-6 ”对 等 计算 模型 


(2) 代理 计算 模型 


如 图 10-7 示 ， 在 每 个 ad-hoc 云 机 器 人 组 中 会 选择 一 个 智能 机 器 人 作为 一 个 主 节 点 ， 用 来 实现 与 云 平 台中 的 代理 (虚拟 设备 ) 进行 通信 ， 从 而 形成 机 器 人 ad-hoc 云 和 云 平台 之 间 的 通信 桥梁 。 通 信 单 元 
被 组 织 为 两 层 结 构 。 这 种 模式 下 的 互 操 作 性 更 强 ， 但 系统 的 鲁 棒 性 较 差 ， 如 果 主 节点 或 者 代理 出 现 故 障 ， 将 会 导致 整 组 无 法 正常 活动 。 





图 10-7 代理 计算 模型 
(3) 基于 克隆 的 计算 模型 


如 图 10-8 所 示 ， 每 一 个 智能 机 器 人 在 云 平台 中 均 有 一 个 对 应 的 系统 克隆 对 象 (虚拟 设备 ) 。 一 个 任务 可 以 在 智能 机 器 人 端 执 行 ， 也 可 以 在 克隆 对 象 端 执行 。 智 能 机 器 人 克隆 对 象 的 集合 采用 点 对 点 的 网 
络 结构 ， 相 比 物理 层 的 ad-hoc M2M 网 络 连 通 性 更 好 。 此 外 ， 该 模型 允许 物理 层 M2M 网 络 零散 地 停机 ， 因 而 该 模型 具有 更 高 的 鲁 棒 性 。 





图 10-8 基于 克隆 的 计算 模型 


10.3.3 ”Rapyuta 机 器 人 云 平台 
Web 应 用 程序 和 机 器 人 应 用 程序 不 同 ， 所 以 现 有 的 通用 Paas 平 台 不 适用 于 机 器 人 场景 。 通 常 Web 应 用 程序 是 无 状态 、 单 进程 的 ， 使 用 请 求 响应 模型 与 客户 端 交 互 。 而 机 器 人 应 用 程序 是 有 状态 的 、 多 
进程 的 ， 并 且 与 客户 端 需要 双向 通信 。 因 而 面向 Web 和 机 器 人 应 用 程序 应 采用 不 同 的 软件 解决 方案 。 


开源 工程 Rapyuta 旨 在 构建 一 个 完整 的 云 机 器 人 平台 ， 并 解决 所 面临 的 挑战 性 问题 。Rapyuta 基 于 伸缩 的 计算 单元 ， 主 要 为 机 器 人 动态 分 配 安全 的 计算 单元 或 镜像 。 这 些 计算 单 元 是 紧密 互联 的 ， 从 而 
允许 机 器 人 共享 知识 和 信息 ; 计算 单元 的 互联 使 得 Rapyuta 成 为 一 个 有 助 于 多 机 器 人 部 署 的 平台 。 此 外 ，Rapyuta 的 计算 单元 提供 了 RoboEarth 知 识 库 的 高 带宽 访问 接口 ， 从 而 使 得 机 器 人 可 以 利用 其 他 机 
器 人 的 经 验 。 


云 机 器 人 出 现 之 前 ， 机 器 人 所 有 的 处 理 、 规 划 和 数据 知识 推理 均 由 机 器 人 自身 完成 ; 利用 Rapyuta， 通 过 部 署 相应 的 代理 或 镜像 ， 便 可 在 云 上 实现 上 述 任务 ， 从 而 减轻 本 地 机 器 人 的 计算 负载 ， 拓 展 本 
地 机 器 人 的 能 力 。 


Rapyuta 计 算 单 元 在 设计 上 考虑 了 ROS 的 适应 性 ， 从 而 使 得 开源 的 ROS 包 可 以 不 做 修改 ， 直 接 运行 ， 极 大 地 方便 了 机 器 人 应 用 程序 的 部 署 。 除 了 对 ROS 的 适应 性 ，Rapyuta 也 可 为 其 他 机 器 人 定制 相应 


的 中 间 件 。 
Rapyuta 采 用 了 Websocket 的 通信 方式 ， 这 就 使 得 云 和 机 器 人 之 间 可 进行 全 双 工 通信 ， 机 器 人 可 以 向 云 服 务 器 发 送 通信 请 求 ， 云 也 可 以 发 送 数据 和 命令 到 机 器 人 。 
1. 主 要 构件 
Rapyuta 主 要 包括 以 下 构件 : 计算 单元 、 通 信 协 议 、 负 责 管理 系统 的 四 个 核心 任务 集 ， 以 及 用 来 组 织 系统 管理 的 命令 数据 框架 ， 以 下 主要 介绍 前 三 个 构件 。 
(1) 计算 单元 


Rapyuta 的 计算 单元 利用 LXC (Linux Container，Linux 容 器 ) 实现 ， 它 为 进程 的 隔离 、 安 全 和 扩展 性 提供 了 一 个 轻 量 级 可 定制 的 解决 方案 。 原 则 上 ，LXC 可 以 被 认为 是 chroot 的 一 个 扩展 版 本 ， 其 在 主 
机 中 对 进程 和 系统 资源 实现 了 隔离 。 由 于 LXC 不 仿真 硬件 ， 同 时 所 有 的 进程 共享 主机 所 提供 的 同一 个 内 核 ， 所 以 应 用 程序 都 以 相似 的 速度 运行 。 此 外 ，LXC 人 允许 对 磁盘 空间 分 配 、 内 存 限制 、1O 速 度 限制 、 
CPU 使 用 率 等 参数 进行 简单 的 设置 ， 从 而 使 得 一 个 环境 可 以 根据 具体 的 需求 进行 配置 ， 实 现 按 需 分 配 计算 资源 。 


每 一 个 计算 单元 可 以 通过 配置 来 运行 任何 进程 ， 如 ROS 节 点 ， 并 且 在 单个 环境 中 ， 利 用 ROS 内 部 通信 机 制 可 实现 所 有 进程 间 的 通信 。 同 时 计算 单元 拥有 完整 的 ROS 运 行 环境 ， 这 就 允许 在 其 中 运行 现 有 
的 ROS 包 ， 从 而 大 大 降低 应 用 程序 开发 者 的 门槛 。 


(2) 通信 协议 
Rapyuta 使 用 EndPoint 进 程 (EP) 定义 云 平台 和 外 部 的 通信 接口 ，EP 间 通过 Ports 实 现 直 接 互联 ， 如 图 10-9 所 示 。 


在 Rapyuta 进 程 和 非 Rapyuta 进 程 之 间 ， 利 用 接口 Interface (1) 进行 通信 ， 非 Rapyuta 进 程 通常 包括 运行 在 机 器 人 端 和 运行 在 计算 单元 中 的 进程 ， 如 图 10-9 中 的 Robot 和 ROS Node。lnterface 采 用 基 
于 服务 的 同步 或 者 基于 主题 的 异步 传输 机 制 进行 通信 。 通 常 ， 机 器 人 和 云 平 台 通信 接口 Interface 包 括 数据 转换 器 ， 负 责 实 现 内 部 数据 消息 与 外 部 通信 格式 之 间 的 转换 。 而 Ports 主 要 用 于 Rapyuta 进 程 间 的 通 


信 。 


Rapyuta ， 
Master WFP 


Task Set 
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图 10-9 ”构件 和 Rapyuta 间 的 基本 通信 通道 


Rapyuta 通 信 协 议 由 三 部 分 组 成 ， 第 一 部 分 是 内 部 通信 协议 ， 它 涵盖 了 Rapyuta 进 程 间 的 所 有 通信 。 第 二 部 分 是 外 部 通信 协议 ， 其 定义 了 物理 机 器 人 和 运行 Rapyuta 的 云 基础 设施 间 的 数据 转换 。 第 三 部 
分 由 Rapyuta 和 运行 在 LXC 中 的 应 用 间 的 通信 构成 。 


(3) 核心 任务 集 
一 个 任务 集 就 是 一 组 功能 集 ， 这 些 任务 集 也 可 以 放 在 一 起 当 作 进程 来 运行 ， 这 依赖 于 具体 的 用 例 。 以 下 为 管理 系统 的 4 个 Rapyuta 任 务 集 。 
1) Master 任 务 集 。 
Master 任 务 集 是 主要 的 控制 任务 ， 用 来 监视 和 维护 以 下 命令 数据 结构 : 
. 机 器 人 和 Rapyuta 间 连接 的 组 织 。 
.来自 机 器 人 所 有 配置 请 求 的 处 理 。 
其 他 任务 集 的 监视 网 络 。 
与 运行 其 他 任务 集 不 同 ， 在 Rapyuta 平 台中 只 运行 一 个 Master 任 务 集 进程 。 
2) 机 器 人 任务 集 。 
根据 任务 集 和 机 器 人 间 通 信 所 必需 的 能 力 来 定义 。 它 包括 : 


- 转发 配置 请 求 到 Master。 


. 数据 消息 的 转换 。 
- 机 器 人 和 其 他 Endpoint 通 信 。 


3) 环境 任务 集 。 


环境 任务 集 定义 了 Rapyuta 框 架 和 计算 单元 间 必 要 的 通信 和 能力， 主要 包括 : 


- 与 ROS 节 点 、 其 他 Endpoint 间 的 通信 。 
启动 /停止 ROS 节 点 。 
. 添加 / 移 除 ROS 参 数 。 
环境 任务 集 的 进程 通常 运行 在 对 应 的 计算 单元 中 。 
描述 启动 、 停 止 计 算 单 元 的 能 力 。 在 每 个 机 器 内 部 运行 包含 容器 任务 集 的 进程 。 容 器 任务 集 主 要 负责 容器 的 动态 创建 与 销毁 
2. 部 署 模型 
前 述 的 核心 组 件 和 通信 协议 可 以 针对 不 同 的 机 器 人 场景 特性 ， 采 用 不 同 的 组 合 方式 。 


图 10-10 所 示 是 PasS 框 架 的 Rapyuta 用 例 部 署 图 。 
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图 10-10 Paas 框 架 的 Rapyuta 用 例 部 署 图 


Master Task set 是 云 3 引擎 框架 的 “主要 大 脑 ”， 来 自用 户 的 每 个 管理 请 求 都 会 被 该 进程 处 理 ， 它 负责 系统 必要 变更 的 启动 流程 。 


Container Task Set 用 来 按照 Master 进 程 的 指令 创建 和 销毁 计算 环境 。 


Robot EP 管理 对 于 机 器 人 的 连接 ， 以 及 将 传 入 或 者 传 出 的 消息 转换 为 合适 的 格式 .。 


Environment EP 进程 负责 ROS 节 点 的 管理 、 运 行 环境 的 配置 。 
另外 两 个 主机 用 来 部 署 容器 ， 在 每 个 包含 容器 的 主机 中 ， 运 行 一 个 单 进程 的 机 器 人 任务 


高 带宽 的 访问 方式 。 


可 以 看 到 ， 云 平台 中 各 个 LXC 之 间 是 可 以 互联 的 ， 其 中 Master 任 务 集 在 其 中 一 个 主机 上 作为 一 个 单 进程 运行 ， 
图 10-10 右 侧 放 大 的 是 LXC 所 拥有 的 计算 环境 ， 虚 线 箭头 暗示 到 同一 个 艇 /数据 中 心 的 RoboEarth 知 识 库 的 连接 ， 这 种 方式 也 提供 了 


一 /一 


， 在 每 个 容器 中 ， 环 境 任务 集 作为 单 进程 运行 。 





1. 智 能 机 器 人 任务 优化 部 署 
寺 不 同性 能 指标 ， 可 以 将 任务 划分 到 云 和 本 地 两 个 子 集中 ， 对 于 既 可 以 部 署 在 本 地 


在 云 机 器 人 环境 下 ， 任 务 部 署 方式 可 以 分 为 本 地 、 云 上 以 及 云 和 本 地 同时 部 署 三 种 方式 ; 按照 实时 性 、 成 本 、 准 确 率 等 不 后 
又 可 以 部 署 在 云 平台 上 的 任务 ， 需 要 进一步 考虑 端 云 同 时 运行 结果 择优 、 基 于 指标 的 端 云 任 务 动态 切换 和 端 云 同 时 运行 结果 融合 等 不 同情 景 。 总 之 根据 不 同 的 应 用 任务 需求 与 特点 ， 基 于 不 同 部 署 方式 特 


征 ， 选 择 合适 方式 ， 使 得 云 机 器 人 应 用 系统 性 能 达到 最 优化 。 


2. 云 机 器 人 资源 分 配 与 调度 方法 
8 监测 控制 设备 按 需 获取 服务 的 需求 ， 将 云端 计算 、 存 储 以 及 数据 、 软 件 等 资源 统一 以 服务 的 形式 实施 封装 和 接 入 ， 使 其 在 物理 上 保持 分 布 自治 的 同时 通过 容器 等 手段 实现 逻 


针对 智能 机 器 人 及 各 类 智 有 
J 资源 的 优化 选取 、 按 需 分 配 和 高 效 访 问 。 


辑 上 的 分 离 管理 ， 并 以 透明 的 方式 进 和 


3. 云 机 器 人 分 布 式 控制 与 决策 方法 
面向 云 架 构 的 智能 机 器 人 系统 要 应 对 复杂 控制 与 应 用 问题 ， 构 建 分 布 式 智能 控制 模型 ， 以 动态 决策 问题 为 研究 对 象 ， 针 对 动态 运动 路 径 规 划 、 高 精度 协同 抓 取 、 多 智能 机 器 人 协同 装配 等 具体 问题 寻找 


优化 控制 与 决策 方法 ， 进 而 设计 面向 云 架 构 的 分 布 式 智能 控制 与 决策 服务 软件 。 


4. 机 器 人 云 服 务 可 信保 障 及 其 评价 


机 器 人 云 服务 可 信保 障 不 仅 涉及 服务 质量 ， 而 且 与 服务 安全 相关 。 服 务 质量 可 采用 Qos 服 务 模型 及 其 区 分 服务 模型 Diff-serv 等 ， 依 据 智能 机 器 人 应 用 的 实时 性 与 可 靠 性 等 要 求 ， 不 仅 在 云端 实施 实时 容 
错 服务 ， 而 且 对 云 机 器 人 网 络 的 接口 拥塞 、 传 输出 错 、 丢 失 数 据 等 采取 数据 流 分 类 ， 流 量 监管 和 控制 等 方法 实施 相应 处 理 ， 提 高 网 络 通 信 的 可 预测 性 和 稳定 性 。 机 器 人 云 服务 安全 则 需要 考虑 高 效 的 身份 认 
证 和 数据 加 密 方法 。 
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